rviz_visual_tools: publishTrajectoryLine seems to be rotated and translated but correct
I have been using moveit to generate my tool path and then wanted to visualize the tool path before I execute it. publishTrajectoryLine from rviz_visual_tools seems to do exactly that. I have been following the tutorial from moveit (http://docs.ros.org/kinetic/api/movei...) but integrating it in my node with a UR10e. When I determine the trajectory and then display the path with publishTrajectoryLine as a line and with MotionPlanning -> Planned Path -> Show Trail, the trail of the TCP of the robot and the line do not match. However the line has a shape very similar to the expected line, but I think it was rotated and translated before displayed. Here a screenshot:
Did I somehow choose the wrong origin for the publishTrajectoryLine command?
Thanks!
Daniel
Below is part of the code which may be relevant:
namespace rvt = rviz_visual_tools;
// For visualizing things in rviz
rviz_visual_tools::RvizVisualToolsPtr visual_tools_;
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.5;
move_group.setPoseTarget(target_pose1);
// Now, we call the planner to compute the plan and visualize it.
// Note that we are just planning, not asking move_group
// to actually move the robot.
// moveit::planning_interface::MoveGroupInterface::Plan my_plan;
/*bool*/ success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
// Visualizing plans
// ^^^^^^^^^^^^^^^^^
// We can also visualize the plan as a line with markers in RViz.
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(target_pose1, "pose2");
visual_tools.publishText(text_pose, "Pose Goal2", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
ros::Duration(0.5).sleep();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
you do now, so please post the screenshot.
Thanks! I edited my question and added the screenshot.