From d725017879d39e6cfbffd83b1ac9b196b8ab6bec Mon Sep 17 00:00:00 2001 From: Emanuele Date: Thu, 14 Nov 2019 15:46:18 +0100 Subject: [PATCH] Plot del percorso con reference frame corrente e non world --- src/roboglue_ros/src/roboglue_recorder.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 78f66b0..9d8c8f9 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/src/roboglue_ros/src/roboglue_recorder.cpp @@ -329,7 +329,7 @@ int main(int argc, char **argv) { ROS_INFO("IK calc in Frame: %s", robot_data.kine_model->getModelFrame().c_str()); /// setup rviz palnned path visualization - rviz_data.vtools = std::make_shared("base_link", "rviz_visual_tools", robot_data.kine_model); + rviz_data.vtools = std::make_shared("world", "rviz_visual_tools", robot_data.kine_model); bool startCycle = true; while (ros::ok() && is.isRunning){ @@ -365,6 +365,7 @@ int main(int argc, char **argv) { frameRot.setRPY(0,0,0); planningFrame.setRotation(frameRot); tfBroadcaster.sendTransform(tf::StampedTransform(planningFrame, ros::Time::now(), "world", "planning_frame")); + rviz_data.vtools->setBaseFrame("planning_frame"); } move_group.setPoseReferenceFrame("planning_frame");