Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10085] pkg/trunk
|
From: <vij...@us...> - 2009-01-23 22:36:15
|
Revision: 10085
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10085&view=rev
Author: vijaypradeep
Date: 2009-01-23 21:42:00 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
Point cloud snapshotter now ignores scans while transitioning between profiles
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2009-01-23 21:41:24 UTC (rev 10084)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2009-01-23 21:42:00 UTC (rev 10085)
@@ -68,7 +68,7 @@
FIRST_HALF = 1, //!< Specifies that we're in the first half of our current profile
SECOND_HALF = 2 //!< Specifies that we're in the second half of our current profile
} ;
-
+
/*!
* \brief Default Constructor of the JointController class.
*
@@ -170,7 +170,7 @@
* Will return NotApplicable if we're not currently following a profile
*/
ProfileExecutionState getProfileExecutionState() ;
-
+
double getTime();
private:
/*!
@@ -221,7 +221,7 @@
*
*/
LaserScannerControllerNode();
-
+
/*!
* \brief Destructor
*/
@@ -253,7 +253,10 @@
* \brief A pointer to ros node
*/
ros::Node *node_;
-
+
+ //! True if a profile was just initiated, and it has not yet hit either end of it's traj
+ bool first_time_ ;
+
LaserScannerController::ProfileExecutionState prev_profile_exec_state_ ; //!< Store the previous profileExecutionState. Need this to compare to the current state to detect transitions
pr2_mechanism_controllers::LaserScannerSignal m_scanner_signal_ ; //!< Stores the message that we want to send at the end of each sweep, and halfway through each sweep
bool need_to_send_msg_ ; //!< Tracks whether we still need to send out the m_scanner_signal_ message.
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2009-01-23 21:41:24 UTC (rev 10084)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2009-01-23 21:42:00 UTC (rev 10085)
@@ -463,7 +463,13 @@
case LaserScannerController::FIRST_HALF :
if (cur_profile_exec_state == LaserScannerController::SECOND_HALF) // We just transitioned from 1st->2nd, so send msg saying half-scan is done
{
- m_scanner_signal_.signal = 0 ; // 0 -> Half profile complete
+ if (first_time_)
+ {
+ m_scanner_signal_.signal = 128 ;
+ first_time_ = false ;
+ }
+ else
+ m_scanner_signal_.signal = 0 ; // 0 -> Half profile complete
//if (need_to_send_msg_) // Is there a message we were supposed to send, but never got to?
// printf("LaserScannerController:: Missed sending a msg\n") ; // Is there a better way to output this error msg?
@@ -474,7 +480,13 @@
case LaserScannerController::SECOND_HALF : // Send msg saying full scan is done
if (cur_profile_exec_state == LaserScannerController::FIRST_HALF) // We just transitioned from 2nd->1st, so send msg saying full-scan is done
{
- m_scanner_signal_.signal = 1 ; // 1 -> Full profile complete
+ if(first_time_)
+ {
+ m_scanner_signal_.signal = 129 ;
+ first_time_ = false ;
+ }
+ else
+ m_scanner_signal_.signal = 1 ; // 1 -> Full profile complete
//if (need_to_send_msg_) // Is there a message we were supposed to send, but never got to?
// printf("LaserScannerController:: Missed sending a msg\n") ; // Is there a better way to output this error msg?
@@ -541,6 +553,8 @@
{
const double num_elem = -1.0 ; // We should only be using the dynamicProfiles, so we don't need num_elem
+ first_time_ = true ;
+
setProfile(LaserScannerController::LaserControllerMode(req.profile),req.period,req.amplitude,num_elem,req.offset);
resp.time = c_->getTime();
return true;
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2009-01-23 21:41:24 UTC (rev 10084)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2009-01-23 21:42:00 UTC (rev 10085)
@@ -83,6 +83,10 @@
void scannerSignalCallback()
{
+ if (cur_signal_.signal == 128 || cur_signal_.signal == 129) // These codes imply that this is the first signal during a given profile type
+ first_time_ = true ;
+
+
if (first_time_)
{
prev_signal_ = cur_signal_ ;
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml 2009-01-23 21:41:24 UTC (rev 10084)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml 2009-01-23 21:42:00 UTC (rev 10085)
@@ -15,8 +15,7 @@
<depend package="pr2_etherCAT"/>
<depend package="pr2_power_board"/>
<depend package="ocean_battery_driver"/>
- <depend package="joy"/>
<depend package="rosrecord"/>
<depend package="hokuyo_node"/>
- <depend package="imu_node"/>
+ <depend package="point_cloud_assembler" />
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
