diff --git a/doc/move_group_interface/launch/move_group.launch.py b/doc/move_group_interface/launch/move_group.launch.py index 242d9f229d..d425eecbc1 100644 --- a/doc/move_group_interface/launch/move_group.launch.py +++ b/doc/move_group_interface/launch/move_group.launch.py @@ -159,7 +159,11 @@ def generate_launch_description(): # Load controllers load_controllers = [] - for controller in ["panda_arm_controller", "joint_state_controller"]: + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_controller", + ]: load_controllers += [ ExecuteProcess( cmd=["ros2 run controller_manager spawner.py {}".format(controller)], diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index f7044c0db1..fbd741a776 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -114,7 +114,7 @@ int main(int argc, char** argv) // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); text_pose.translation().z() = 1.0; - visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE); // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations visual_tools.trigger(); @@ -165,7 +165,7 @@ int main(int argc, char** argv) // We can also visualize the plan as a line with markers in RViz. RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); visual_tools.publishAxisLabeled(target_pose1, "pose1"); - visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Pose_Goal", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); prompt("Press 'Enter' to continue the demo"); @@ -215,7 +215,7 @@ int main(int argc, char** argv) // Visualize the plan in RViz visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); prompt("Press 'Enter' to continue the demo"); @@ -285,7 +285,7 @@ int main(int argc, char** argv) visual_tools.deleteAllMarkers(); visual_tools.publishAxisLabeled(start_pose2, "start"); visual_tools.publishAxisLabeled(target_pose1, "goal"); - visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Constrained_Goal", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); prompt("Press 'Enter' to continue the demo"); @@ -329,7 +329,7 @@ int main(int argc, char** argv) // Visualize the plan in RViz visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Cartesian_Path", rvt::WHITE, rvt::XLARGE); visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL); for (std::size_t i = 0; i < waypoints.size(); ++i) visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); @@ -362,7 +362,7 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED"); visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(another_pose, "goal"); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); @@ -409,7 +409,7 @@ int main(int argc, char** argv) planning_scene_interface.addCollisionObjects(collision_objects); // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message - visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Add_object", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); prompt("Press 'Enter' to continue once the collision object appears in RViz"); /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); */ @@ -417,7 +417,7 @@ int main(int argc, char** argv) // Now when we plan a trajectory it will avoid the obstacle success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); - visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Obstacle_Goal", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); prompt("Press 'Enter' to continue once the plan is complete"); @@ -464,7 +464,7 @@ int main(int argc, char** argv) touch_links.push_back("panda_leftfinger"); move_group.attachObject(object_to_attach.id, "panda_hand", touch_links); - visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ @@ -494,7 +494,7 @@ int main(int argc, char** argv) // Show text in RViz of status visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ @@ -509,7 +509,7 @@ int main(int argc, char** argv) planning_scene_interface.removeCollisionObjects(object_ids); // Show text in RViz of status - visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ @@ -517,6 +517,8 @@ int main(int argc, char** argv) /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears"); */ // END_TUTORIAL + visual_tools.deleteAllMarkers(); + visual_tools.trigger(); rclcpp::shutdown(); return 0; diff --git a/doc/moveit_cpp/images/moveitcpp_plan5.png b/doc/moveit_cpp/images/moveitcpp_plan5.png new file mode 100644 index 0000000000..b7914a5aba Binary files /dev/null and b/doc/moveit_cpp/images/moveitcpp_plan5.png differ diff --git a/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py b/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py index d9a66ca26b..062b29c0d6 100644 --- a/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py +++ b/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py @@ -55,7 +55,6 @@ def generate_launch_description(): kinematics_yaml = load_yaml( "moveit_resources_panda_moveit_config", "config/kinematics.yaml" ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} moveit_simple_controllers_yaml = load_yaml( "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml" @@ -144,7 +143,11 @@ def generate_launch_description(): # Load controllers load_controllers = [] - for controller in ["panda_arm_controller", "joint_state_controller"]: + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_controller", + ]: load_controllers += [ ExecuteProcess( cmd=["ros2 run controller_manager spawner.py {}".format(controller)], diff --git a/doc/moveit_cpp/launch/moveit_cpp_tutorial.rviz b/doc/moveit_cpp/launch/moveit_cpp_tutorial.rviz index ae1cef7629..68fc6df7dd 100644 --- a/doc/moveit_cpp/launch/moveit_cpp_tutorial.rviz +++ b/doc/moveit_cpp/launch/moveit_cpp_tutorial.rviz @@ -48,7 +48,7 @@ Visualization Manager: Enabled: true Move Group Namespace: "" Name: PlanningScene - Planning Scene Topic: /moveit_cpp/publish_planning_scene + Planning Scene Topic: /moveit_cpp/monitored_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 0.8999999761581421 diff --git a/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp b/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp index 712dc43fbd..3dc1c4b0f8 100644 --- a/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp +++ b/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp @@ -56,11 +56,10 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials..."); - auto moveit_cpp_ptr = std::make_shared(node); + auto moveit_cpp_ptr = std::make_shared(node); moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService(); - auto planning_components = - std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); + auto planning_components = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); auto robot_model_ptr = moveit_cpp_ptr->getRobotModel(); auto robot_start_state = planning_components->getStartState(); auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); @@ -78,7 +77,7 @@ int main(int argc, char** argv) Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); text_pose.translation().z() = 1.75; - visual_tools.publishText(text_pose, "MoveItCpp Demo", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "MoveItCpp_Demo", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); // Start the demo @@ -115,10 +114,9 @@ int main(int argc, char** argv) { // Visualize the start pose in Rviz visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "start_pose"); - visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE); // Visualize the goal pose in Rviz visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose"); - visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "setStartStateToCurrentState", rvt::WHITE, rvt::XLARGE); // Visualize the trajectory in Rviz /* visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); @@ -134,9 +132,10 @@ int main(int argc, char** argv) // :align: center // // Start the next plan - visual_tools.deleteAllMarkers(); prompt("Press 'Enter' to continue the demo"); /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ + visual_tools.deleteAllMarkers(); + visual_tools.trigger(); // Plan #2 // ^^^^^^^ @@ -161,10 +160,9 @@ int main(int argc, char** argv) moveit::core::RobotState robot_state(robot_model_ptr); moveit::core::robotStateMsgToRobotState(plan_solution2.start_state, robot_state); - visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose"); - visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose"); + visual_tools.publishText(text_pose, "moveit::core::RobotState_Start_State", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); @@ -179,9 +177,10 @@ int main(int argc, char** argv) // :align: center // // Start the next plan - visual_tools.deleteAllMarkers(); prompt("Press 'Enter' to continue the demo"); /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ + visual_tools.deleteAllMarkers(); + visual_tools.trigger(); // Plan #3 // ^^^^^^^ @@ -206,10 +205,9 @@ int main(int argc, char** argv) moveit::core::RobotState robot_state(robot_model_ptr); moveit::core::robotStateMsgToRobotState(plan_solution3.start_state, robot_state); - visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose"); - visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(target_pose2, "target_pose"); + visual_tools.publishText(text_pose, "moveit::core::RobotState_Goal_Pose", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); @@ -224,9 +222,10 @@ int main(int argc, char** argv) // :align: center // // Start the next plan - visual_tools.deleteAllMarkers(); prompt("Press 'Enter' to continue the demo"); /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ + visual_tools.deleteAllMarkers(); + visual_tools.trigger(); // Plan #4 // ^^^^^^^ @@ -249,10 +248,9 @@ int main(int argc, char** argv) moveit::core::RobotState robot_state(robot_model_ptr); moveit::core::robotStateMsgToRobotState(plan_solution4.start_state, robot_state); - visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose"); - visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "target_pose"); + visual_tools.publishText(text_pose, "Goal_Pose_From_Named_State", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); @@ -266,10 +264,65 @@ int main(int argc, char** argv) // :width: 250pt // :align: center // - // END_TUTORIAL + // Start the next plan + prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ visual_tools.deleteAllMarkers(); + visual_tools.trigger(); + + // Plan #5 + // ^^^^^^^ + // + // We can also generate motion plans around objects in the collision scene. + // + // First we create the collision object + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = "panda_link0"; + collision_object.id = "box"; + + shape_msgs::msg::SolidPrimitive box; + box.type = box.BOX; + box.dimensions = { 0.1, 0.4, 0.1 }; + + geometry_msgs::msg::Pose box_pose; + box_pose.position.x = 0.4; + box_pose.position.y = 0.0; + box_pose.position.z = 1.0; + + collision_object.primitives.push_back(box); + collision_object.primitive_poses.push_back(box_pose); + collision_object.operation = collision_object.ADD; + + // Add object to planning scene + { // Lock PlanningScene + planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr->getPlanningSceneMonitor()); + scene->processCollisionObjectMsg(collision_object); + } // Unlock PlanningScene + planning_components->setStartStateToCurrentState(); + planning_components->setGoal("extended"); + + auto plan_solution5 = planning_components->plan(); + if (plan_solution5) + { + visual_tools.publishText(text_pose, "Planning_Around_Collision_Object", rvt::WHITE, rvt::XLARGE); + /* visual_tools.publishTrajectoryLine(plan_solution5.trajectory, joint_model_group_ptr); */ + visual_tools.trigger(); + + /* Uncomment if you want to execute the plan */ + /* planning_components->execute(); // Execute the plan */ + } + + // Plan #5 visualization: + // + // .. image:: images/moveitcpp_plan5.png + // :width: 250pt + // :align: center + // + // END_TUTORIAL prompt("Press 'Enter' to exit the demo"); /* visual_tools.prompt("Press 'next' to end the demo"); */ + visual_tools.deleteAllMarkers(); + visual_tools.trigger(); RCLCPP_INFO(LOGGER, "Shutting down."); rclcpp::shutdown(); diff --git a/doc/quickstart_in_rviz/launch/demo.launch.py b/doc/quickstart_in_rviz/launch/demo.launch.py index 55090b6790..fb079864dc 100644 --- a/doc/quickstart_in_rviz/launch/demo.launch.py +++ b/doc/quickstart_in_rviz/launch/demo.launch.py @@ -183,7 +183,11 @@ def generate_launch_description(): # Load controllers load_controllers = [] - for controller in ["panda_arm_controller", "joint_state_controller"]: + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_controller", + ]: load_controllers += [ ExecuteProcess( cmd=["ros2 run controller_manager spawner.py {}".format(controller)],