Allow choosing between geometry_msgs/Twist and

geometry_msgs/TwistStamped (issue CCNYRoboticsLab#21)
corot · May 17, 2015 · 934155c · 934155c
1 parent b5efb32
commit 934155c
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 10 deletions.
diff --git a/laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher.h b/laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher.h
@@ -118,6 +118,7 @@ class LaserScanMatcher
     bool use_imu_;
     bool use_odom_;
     bool use_vel_;
+    bool stamped_vel_;
 
     // **** state variables
 
@@ -138,7 +139,7 @@ class LaserScanMatcher
     nav_msgs::Odometry latest_odom_msg_;
     nav_msgs::Odometry last_used_odom_msg_;
 
-    geometry_msgs::TwistStamped latest_vel_msg_;
+    geometry_msgs::Twist latest_vel_msg_;
 
     std::vector<double> a_cos_;
     std::vector<double> a_sin_;
@@ -162,7 +163,8 @@ class LaserScanMatcher
 
     void odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg);
     void imuCallback (const sensor_msgs::Imu::ConstPtr& imu_msg);
-    void velCallback (const geometry_msgs::TwistStamped::ConstPtr& twist_msg);
+    void velCallback (const geometry_msgs::Twist::ConstPtr& twist_msg);
+    void velStmpCallback(const geometry_msgs::TwistStamped::ConstPtr& twist_msg);
 
     void createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg);
     bool getBaseToLaserTf (const std::string& frame_id);

diff --git a/laser_scan_matcher/src/laser_scan_matcher.cpp b/laser_scan_matcher/src/laser_scan_matcher.cpp
@@ -102,8 +102,12 @@ LaserScanMatcher::LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_privat
   }
   if (use_vel_)
   {
-    vel_subscriber_ = nh_.subscribe(
-      "vel", 1, &LaserScanMatcher::velCallback, this);
+    if (stamped_vel_)
+      vel_subscriber_ = nh_.subscribe(
+        "vel", 1, &LaserScanMatcher::velStmpCallback, this);
+    else
+      vel_subscriber_ = nh_.subscribe(
+        "vel", 1, &LaserScanMatcher::velCallback, this);
   }
 }
 
@@ -120,8 +124,8 @@ void LaserScanMatcher::initParams()
     fixed_frame_ = "world";
 
   // **** input type - laser scan, or point clouds?
-  // if false, will subscrive to LaserScan msgs on /scan.
-  // if true, will subscrive to PointCloud2 msgs on /cloud
+  // if false, will subscribe to LaserScan msgs on /scan.
+  // if true, will subscribe to PointCloud2 msgs on /cloud
 
   if (!nh_private_.getParam ("use_cloud_input", use_cloud_input_))
     use_cloud_input_= false;
@@ -162,6 +166,12 @@ void LaserScanMatcher::initParams()
   if (!nh_private_.getParam ("use_vel", use_vel_))
     use_vel_ = false;
 
+  // **** Are velocity input messages stamped?
+  // if false, will subscribe to Twist msgs on /vel
+  // if true, will subscribe to TwistStamped msgs on /vel
+  if (!nh_private_.getParam ("stamped_vel", stamped_vel_))
+    stamped_vel_= true;
+
   // **** How to publish the output?
   // tf (fixed_frame->base_frame),
   // pose message (pose of base frame in the fixed frame)
@@ -314,14 +324,22 @@ void LaserScanMatcher::odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg
   }
 }
 
-void LaserScanMatcher::velCallback(const geometry_msgs::TwistStamped::ConstPtr& twist_msg)
+void LaserScanMatcher::velCallback(const geometry_msgs::Twist::ConstPtr& twist_msg)
 {
   boost::mutex::scoped_lock(mutex_);
   latest_vel_msg_ = *twist_msg;
 
   received_vel_ = true;
 }
 
+void LaserScanMatcher::velStmpCallback(const geometry_msgs::TwistStamped::ConstPtr& twist_msg)
+{
+  boost::mutex::scoped_lock(mutex_);
+  latest_vel_msg_ = twist_msg->twist;
+
+  received_vel_ = true;
+}
+
 void LaserScanMatcher::cloudCallback (const PointCloudT::ConstPtr& cloud)
 {
   // **** if first scan, cache the tf from base to the scanner
@@ -674,9 +692,9 @@ void LaserScanMatcher::getPrediction(double& pr_ch_x, double& pr_ch_y,
   // **** use velocity (for example from ab-filter)
   if (use_vel_)
   {
-    pr_ch_x = dt * latest_vel_msg_.twist.linear.x;
-    pr_ch_y = dt * latest_vel_msg_.twist.linear.y;
-    pr_ch_a = dt * latest_vel_msg_.twist.angular.z;
+    pr_ch_x = dt * latest_vel_msg_.linear.x;
+    pr_ch_y = dt * latest_vel_msg_.linear.y;
+    pr_ch_a = dt * latest_vel_msg_.angular.z;
 
     if      (pr_ch_a >= M_PI) pr_ch_a -= 2.0 * M_PI;
     else if (pr_ch_a < -M_PI) pr_ch_a += 2.0 * M_PI;