| 320 | } |
| 321 | |
| 322 | bool CostmapInterface::GetRobotPose(tf::Stamped<tf::Pose> &global_pose) const { |
| 323 | global_pose.setIdentity(); |
| 324 | tf::Stamped<tf::Pose> robot_pose; |
| 325 | robot_pose.setIdentity(); |
| 326 | robot_pose.frame_id_ = robot_base_frame_; |
| 327 | robot_pose.stamp_ = ros::Time(); |
| 328 | ros::Time current_time = ros::Time::now(); |
| 329 | try { |
| 330 | tf_.transformPose(global_frame_, robot_pose, global_pose); |
| 331 | } |
| 332 | catch (tf::LookupException &ex) { |
| 333 | ROS_ERROR("No Transform Error looking up robot pose: %s", ex.what()); |
| 334 | return false; |
| 335 | } |
| 336 | catch (tf::ConnectivityException &ex) { |
| 337 | ROS_ERROR("Connectivity Error looking up robot pose: %s", ex.what()); |
| 338 | return false; |
| 339 | } |
| 340 | catch (tf::ExtrapolationException &ex) { |
| 341 | ROS_ERROR("Extrapolation Error looking up robot pose: %s", ex.what()); |
| 342 | return false; |
| 343 | } |
| 344 | // check global_pose timeout |
| 345 | //if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) { |
| 346 | // LOG_WARNING << "Interface transform timeout. Current time: " << current_time.toSec() << ", global_pose stamp: " |
| 347 | // << global_pose.stamp_.toSec() << ", tolerance: " << transform_tolerance_; |
| 348 | // return false; |
| 349 | //} |
| 350 | return true; |
| 351 | } |
| 352 | |
| 353 | bool CostmapInterface::GetRobotPose(geometry_msgs::PoseStamped &global_pose) const { |
| 354 | tf::Stamped<tf::Pose> tf_global_pose; |
no outgoing calls
no test coverage detected