[tf2][Move Base] finish compile error fixes

This finishes the compile errors from cherry-picking #458. This commit ports the changes from #601 to tf2.
ros-planning · Jul 15, 2018 · 8b327b0 · 8b327b0
1 parent 053973a
commit 8b327b0
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 14 deletions.
diff --git a/move_base/include/move_base/move_base.h b/move_base/include/move_base/move_base.h
@@ -164,7 +164,7 @@ namespace move_base {
 
       bool isQuaternionValid(const geometry_msgs::Quaternion& q);
 
-      bool getRobotPose(tf::Stamped<tf::Pose>& global_posee, costmap_2d::Costmap2DROS* costmap);
+      bool getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap);
 
       double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
 

diff --git a/move_base/src/move_base.cpp b/move_base/src/move_base.cpp
@@ -471,7 +471,6 @@ namespace move_base {
     }
 
     //get the starting pose of the robot
-    tf::Stamped<tf::Pose> global_pose;
     geometry_msgs::PoseStamped global_pose;
     if(!getRobotPose(global_pose, planner_costmap_ros_)) {
       ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
@@ -1136,42 +1135,42 @@ namespace move_base {
     }
   }
 
-  bool MoveBase::getRobotPose(tf::Stamped<tf::Pose>& global_pose, costmap_2d::Costmap2DROS* costmap)
+  bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap)
   {
-    global_pose.setIdentity();
-    tf::Stamped < tf::Pose > robot_pose;
-    robot_pose.setIdentity();
-    robot_pose.frame_id_ = robot_base_frame_;
-    robot_pose.stamp_ = ros::Time(); // latest available
+    tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
+    geometry_msgs::PoseStamped robot_pose;
+    tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
+    robot_pose.header.frame_id = robot_base_frame_;
+    robot_pose.header.stamp = ros::Time(); // latest available
     ros::Time current_time = ros::Time::now();  // save time for checking tf delay later
 
     // get robot pose on the given costmap frame
     try
     {
-      tf_.transformPose(costmap->getGlobalFrameID(), robot_pose, global_pose);
+      tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID());
     }
-    catch (tf::LookupException& ex)
+    catch (tf2::LookupException& ex)
     {
       ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
       return false;
     }
-    catch (tf::ConnectivityException& ex)
+    catch (tf2::ConnectivityException& ex)
     {
       ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
       return false;
     }
-    catch (tf::ExtrapolationException& ex)
+    catch (tf2::ExtrapolationException& ex)
     {
       ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
       return false;
     }
 
     // check if global_pose time stamp is within costmap transform tolerance
-    if (current_time.toSec() - global_pose.stamp_.toSec() > costmap->getTransformTolerance())
+    if (current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
     {
       ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. " \
                         "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->getName().c_str(),
-                        current_time.toSec(), global_pose.stamp_.toSec(), costmap->getTransformTolerance());
+                        current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
       return false;
     }