[Personalrobots-commit] SF.net SVN: personalrobots:[18059] pkg/trunk

[<vij...@us...>]

Revision: 18059
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18059&view=rev
Author:   vijaypradeep
Date:     2009-07-01 01:24:04 +0000 (Wed, 01 Jul 2009)

Log Message:
-----------
LaserScannerTrajController now publishes laser_scanner_signal on custom periodic trajectories

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
    pkg/trunk/util/trajectory/include/trajectory/trajectory.h
    pkg/trunk/util/trajectory/src/trajectory.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h	2009-07-01 01:23:31 UTC (rev 18058)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h	2009-07-01 01:24:04 UTC (rev 18059)
@@ -86,6 +86,8 @@
   inline double getCurProfileTime() ;
   //! \brief Returns the length (in seconds) of our current profile
   inline double getProfileDuration() ;
+  //! \brief Returns the current trajectory segment we're executing in our current profile
+  inline int getCurProfileSegment() ;
 
 private:
 
@@ -149,7 +151,7 @@
   mechanism::RobotState *robot_ ;
   std::string service_prefix_ ;
 
-  double prev_profile_time_ ;                                                    //!< The time in the current profile when update() was last called
+  int prev_profile_segment_ ;                                                    //!< The segment in the current profile when update() was last called
 
   pr2_msgs::PeriodicCmd cmd_ ;
   pr2_msgs::LaserTrajCmd traj_cmd_ ;

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp	2009-07-01 01:23:31 UTC (rev 18058)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp	2009-07-01 01:24:04 UTC (rev 18059)
@@ -228,6 +228,12 @@
   return traj_duration_ ;
 }
 
+int LaserScannerTrajController::getCurProfileSegment()
+{
+  double cur_time = getCurProfileTime() ;
+  return traj_.findTrajectorySegment(cur_time) ;
+}
+
 bool LaserScannerTrajController::setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp)
 {
   while (!traj_lock_.try_lock())
@@ -404,6 +410,7 @@
 ROS_REGISTER_CONTROLLER(LaserScannerTrajControllerNode)
 LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): node_(ros::Node::instance()), c_()
 {
+  prev_profile_segment_ = -1 ;
   need_to_send_msg_ = false ;                                           // Haven't completed a sweep yet, so don't need to send a msg
   publisher_ = NULL ;                                                   // We don't know our topic yet, so we can't build it
 }
@@ -428,29 +435,20 @@
 {
   c_.update() ;
 
-  double cur_profile_time = c_.getCurProfileTime() ;
+  int cur_profile_segment = c_.getCurProfileSegment() ;
 
-  // Check if we crossed the middle point of our profile
-  if (cur_profile_time  >= c_.getProfileDuration()/2.0 &&
-      prev_profile_time_ < c_.getProfileDuration()/2.0)
+  if (cur_profile_segment != prev_profile_segment_)
   {
     // Should we be populating header.stamp here? Or, we can simply let ros take care of the timestamp
     ros::Time cur_time ;
     cur_time.fromSec(robot_->hw_->current_time_) ;
     m_scanner_signal_.header.stamp = cur_time ;
-    m_scanner_signal_.signal = 0 ;
+    m_scanner_signal_.signal = cur_profile_segment ;
     need_to_send_msg_ = true ;
   }
-  else if (cur_profile_time < prev_profile_time_)        // Check if we wrapped around
-  {
-    ros::Time cur_time ;
-    cur_time.fromSec(robot_->hw_->current_time_) ;
-    m_scanner_signal_.header.stamp = cur_time ;
-    m_scanner_signal_.signal = 1 ;
-    need_to_send_msg_ = true ;
-  }
-  prev_profile_time_ = cur_profile_time ;
 
+  prev_profile_segment_ = cur_profile_segment ;
+
   // Use the realtime_publisher to try to send the message.
   //   If it fails sending, it's not a big deal, since we can just try again 1 ms later. No one will notice.
   if (need_to_send_msg_)
@@ -462,8 +460,6 @@
       publisher_->unlockAndPublish() ;
       need_to_send_msg_ = false ;
     }
-    //printf("tilt_laser: Signal trigger (%u)\n", m_scanner_signal_.signal) ;
-    //std::cout << std::flush ;
   }
 }
 
@@ -491,7 +487,7 @@
     delete publisher_ ;
   publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
 
-  prev_profile_time_ = 0.0 ;
+  prev_profile_segment_ = -1 ;
 
   ROS_INFO("Successfully spawned %s", service_prefix_.c_str()) ;
 
@@ -508,6 +504,7 @@
   else
   {
     res.start_time = ros::Time::now();
+    prev_profile_segment_ = -1 ;
     return true;
   }
 }
@@ -515,6 +512,7 @@
 void LaserScannerTrajControllerNode::setPeriodicCmd()
 {
   c_.setPeriodicCmd(cmd_) ;
+  prev_profile_segment_ = -1 ;
 }
 
 bool LaserScannerTrajControllerNode::setTrajSrv(pr2_srvs::SetLaserTrajCmd::Request &req,
@@ -527,6 +525,7 @@
   else
   {
     res.start_time = ros::Time::now();
+    prev_profile_segment_ = -1 ;
     return true;
   }
 }
@@ -535,6 +534,7 @@
 void LaserScannerTrajControllerNode::setTrajCmd()
 {
   c_.setTrajCmd(traj_cmd_) ;
+  prev_profile_segment_ = -1 ;
 }
 
 /*void LaserScannerTrajControllerNode::setTrackLinkCmd()

Modified: pkg/trunk/util/trajectory/include/trajectory/trajectory.h
===================================================================
--- pkg/trunk/util/trajectory/include/trajectory/trajectory.h	2009-07-01 01:23:31 UTC (rev 18058)
+++ pkg/trunk/util/trajectory/include/trajectory/trajectory.h	2009-07-01 01:24:04 UTC (rev 18059)
@@ -211,6 +211,14 @@
 
     void setJointWraps(int index);
 
+    /*! 
+       \brief finds the trajectory segment corresponding to a particular time 
+       \param input time (in seconds)
+       \return segment index 
+    */
+    int findTrajectorySegment(double time);
+
+
     protected:
 
     std::string interp_method_; /** string representation of interpolation method */
@@ -320,13 +328,6 @@
     */
     void sampleBlendedLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time);
 
-    /*! 
-       \brief finds the trajectory segment corresponding to a particular time 
-       \param input time (in seconds)
-       \return segment index 
-    */
-    inline int findTrajectorySegment(double time);
-
     double calculateMinTimeCubic(double q0, double q1, double v0, double v1, double vmax, int index);
 
     /*!

Modified: pkg/trunk/util/trajectory/src/trajectory.cpp
===================================================================
--- pkg/trunk/util/trajectory/src/trajectory.cpp	2009-07-01 01:23:31 UTC (rev 18058)
+++ pkg/trunk/util/trajectory/src/trajectory.cpp	2009-07-01 01:24:04 UTC (rev 18059)
@@ -258,7 +258,7 @@
   parameterize();
 }
 
-inline int Trajectory::findTrajectorySegment(double time)
+int Trajectory::findTrajectorySegment(double time)
 {
   int result = 0;
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.