| 405 | } |
| 406 | |
| 407 | geometry_msgs::PoseStamped CostmapInterface::Pose2GlobalFrame(const geometry_msgs::PoseStamped &pose_msg) { |
| 408 | tf::Stamped<tf::Pose> tf_pose, global_tf_pose; |
| 409 | poseStampedMsgToTF(pose_msg, tf_pose); |
| 410 | |
| 411 | tf_pose.stamp_ = ros::Time(); |
| 412 | try { |
| 413 | tf_.transformPose(global_frame_, tf_pose, global_tf_pose); |
| 414 | } |
| 415 | catch (tf::TransformException &ex) { |
| 416 | return pose_msg; |
| 417 | } |
| 418 | geometry_msgs::PoseStamped global_pose_msg; |
| 419 | tf::poseStampedTFToMsg(global_tf_pose, global_pose_msg); |
| 420 | return global_pose_msg; |
| 421 | } |
| 422 | |
| 423 | void CostmapInterface::ClearCostMap() { |
| 424 | std::vector<Layer *> *plugins = layered_costmap_->GetPlugins(); |