// get plan_to_global_transform from plan frame to global_frame tf::StampedTransform plan_to_global_transform; tf.waitForTransform(global_frame, ros::Time::now(), plan_pose.header.frame_id, plan_pose.header.stamp, plan_pose.header.frame_id, ros::Duration(0.5)); tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp, plan_pose.header.frame_id, plan_to_global_transform);
//let's get the pose of the robot in the frame of the plan tf::Stamped<tf::Pose> robot_pose; tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose);
for (int i = path.size() -1; i>=0; i--) { std::pair<float, float> point = path[i]; //convert the plan to world coordinates double world_x, world_y; mapToWorld(point.first, point.second, world_x, world_y);
geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = global_frame; pose.pose.position.x = world_x; pose.pose.position.y = world_y; pose.pose.position.z = 0.0; pose.pose.orientation.x = 0.0; pose.pose.orientation.y = 0.0; pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; plan.push_back(pose); }