Plot del percorso con reference frame corrente e non world

This commit is contained in:
2019-11-14 15:46:18 +01:00
parent 270bf14a3e
commit d725017879

View File

@@ -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<mvt::MoveItVisualTools>("base_link", "rviz_visual_tools", robot_data.kine_model);
rviz_data.vtools = std::make_shared<mvt::MoveItVisualTools>("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");