diff --git a/CMakeLists.txt b/CMakeLists.txt index 8a5cda451b..a555a5c688 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,7 +54,7 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) # add_subdirectory(doc/interactivity) # add_subdirectory(doc/kinematics) # add_subdirectory(doc/motion_planning_api) -# add_subdirectory(doc/motion_planning_pipeline) +add_subdirectory(doc/motion_planning_pipeline) add_subdirectory(doc/move_group_interface) # add_subdirectory(doc/move_group_python_interface) # add_subdirectory(doc/perception_pipeline) diff --git a/doc/motion_planning_pipeline/CMakeLists.txt b/doc/motion_planning_pipeline/CMakeLists.txt index 8dc42ca3e3..25d9b60336 100644 --- a/doc/motion_planning_pipeline/CMakeLists.txt +++ b/doc/motion_planning_pipeline/CMakeLists.txt @@ -1,8 +1,13 @@ add_executable(motion_planning_pipeline_tutorial src/motion_planning_pipeline_tutorial.cpp) -target_link_libraries(motion_planning_pipeline_tutorial - ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS motion_planning_pipeline_tutorial DESTINATION - ${CATKIN_PACKAGE_BIN_DESTINATION}) +target_include_directories(motion_planning_pipeline_tutorial + PUBLIC include) +ament_target_dependencies(motion_planning_pipeline_tutorial + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(TARGETS motion_planning_pipeline_tutorial + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch b/doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch deleted file mode 100644 index 92b851f89a..0000000000 --- a/doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch.py b/doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch.py new file mode 100644 index 0000000000..de39dacbf1 --- /dev/null +++ b/doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch.py @@ -0,0 +1,66 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # planning_context + robot_description_config = load_file( + "moveit_resources_panda_description", "urdf/panda.urdf" + ) + robot_description = {"robot_description": robot_description_config} + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + motion_planning_pipeline_tutorial = Node( + name="motion_planning_pipeline_tutorial", + package="moveit2_tutorials", + executable="motion_planning_pipeline_tutorial", + prefix="xterm -e", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + {"planning_plugin": "ompl_interface/OMPLPlanner"}, + { + "planning_adapters": "default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints" + }, + {"start_state_max_bounds_error": 0.1}, + ], + ) + + return LaunchDescription([motion_planning_pipeline_tutorial]) diff --git a/doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.rst b/doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.rst index 99ad4a6d54..0a98185722 100644 --- a/doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.rst +++ b/doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.rst @@ -1,8 +1,3 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - Motion Planning Pipeline ================================== @@ -22,15 +17,17 @@ Running the Code ---------------- Open two shells. In the first shell start RViz and wait for everything to finish loading: :: - roslaunch panda_moveit_config demo.launch + ros2 launch moveit_resources_panda_moveit_config demo.launch.py In the second shell, run the launch file: :: - roslaunch moveit_tutorials motion_planning_pipeline_tutorial.launch + ros2 launch moveit2_tutorials motion_planning_pipeline_tutorial.launch.py + +**Note:** Because **MoveitVisualTools** has not been ported to ROS2 this tutoral has made use of xterm and a simple prompter to help the user progress through each demo step. +To install xterm please run the following command: :: -**Note:** This tutorial uses the **RvizVisualToolsGui** panel to step through the demo. To add this panel to RViz, follow the instructions in the `Visualization Tutorial <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools>`_. + sudo apt-get install -y xterm -After a short moment, the RViz window should appear and look similar to the one at the top of this page. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **N** on your keyboard while RViz is focused. Expected Output --------------- @@ -48,4 +45,4 @@ The entire code can be seen :codedir:`here in the MoveIt GitHub project` on GitHub. All the code in this tutorial can be compiled and run from the moveit_tutorials package that you have as part of your MoveIt setup. +The entire launch file is :codedir:`here ` on GitHub. All the code in this tutorial can be compiled and run from the moveit_tutorials package that you have as part of your MoveIt setup. diff --git a/doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp b/doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp index 0885086e0f..11c2173e0e 100644 --- a/doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp +++ b/doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp @@ -34,8 +34,8 @@ /* Author: Sachin Chitta, Mike Lautman*/ -#include -#include +#include +#include // MoveIt #include @@ -44,16 +44,40 @@ #include #include #include -#include -#include -#include +#include +#include +/* #include This has not been ported to ros2 yet */ +#include +/* this is a standin for moveit_visual_tools visual_tools.prompt */ +#include + +void prompt(const std::string& message) +{ + printf(MOVEIT_CONSOLE_COLOR_GREEN "\n%s" MOVEIT_CONSOLE_COLOR_RESET, message.c_str()); + fflush(stdout); + while (std::cin.get() != '\n' && rclcpp::ok()) + ; +} + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("motion_planning_pipeline_tutorial"); int main(int argc, char** argv) { - ros::init(argc, argv, "move_group_tutorial"); - ros::AsyncSpinner spinner(1); - spinner.start(); - ros::NodeHandle node_handle("~"); + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + RCLCPP_INFO(LOGGER, "Initialize node"); + + // This enables loading undeclared parameters + // best practice would be to declare parameters in the corresponding classes + // and provide descriptions about expected use + node_options.automatically_declare_parameters_from_overrides(true); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("motion_planning_pipeline", "", node_options); + + // We spin up a SingleThreadedExecutor for the current state monitor to get information + // about the robot's state. + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread([&executor]() { executor.spin(); }).detach(); // BEGIN_TUTORIAL // Start @@ -67,14 +91,14 @@ int main(int argc, char** argv) // .. _RobotModelLoader: // http://docs.ros.org/noetic/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html robot_model_loader::RobotModelLoaderPtr robot_model_loader( - new robot_model_loader::RobotModelLoader("robot_description")); + new robot_model_loader::RobotModelLoader(node, "robot_description")); // Using the RobotModelLoader, we can construct a planing scene monitor that // will create a planning scene, monitors planning scene diffs, and apply the diffs to it's // internal planning scene. We then call startSceneMonitor, startWorldGeometryMonitor and // startStateMonitor to fully initialize the planning scene monitor planning_scene_monitor::PlanningSceneMonitorPtr psm( - new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader)); + new planning_scene_monitor::PlanningSceneMonitor(node, robot_model_loader)); /* listen for planning scene messages on topic /XXX and apply them to the internal planning scene the internal planning scene accordingly */ @@ -102,19 +126,20 @@ int main(int argc, char** argv) // We can now setup the PlanningPipeline object, which will use the ROS parameter server // to determine the set of request adapters and the planning plugin to use planning_pipeline::PlanningPipelinePtr planning_pipeline( - new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters")); + new planning_pipeline::PlanningPipeline(robot_model, node, "planning_plugin", "request_adapters")); // Visualization // ^^^^^^^^^^^^^ // The package MoveItVisualTools provides many capabilities for visualizing objects, robots, // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script. namespace rvt = rviz_visual_tools; - moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); + /* moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); */ + rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "moveit_cpp_tutorial", node); visual_tools.deleteAllMarkers(); /* Remote control is an introspection tool that allows users to step through a high level script via buttons and keyboard shortcuts in RViz */ - visual_tools.loadRemoteControl(); + // visual_tools.loadRemoteControl(); /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/ Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); @@ -125,7 +150,8 @@ int main(int argc, char** argv) visual_tools.trigger(); /* We can also use visual_tools to wait for user input */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + prompt("Press 'Enter' to start the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); */ // Pose Goal // ^^^^^^^^^ @@ -133,7 +159,7 @@ int main(int argc, char** argv) // specifying the desired pose of the end-effector as input. planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; - geometry_msgs::PoseStamped pose; + geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = "panda_link0"; pose.pose.position.x = 0.3; pose.pose.position.y = 0.0; @@ -153,7 +179,7 @@ int main(int argc, char** argv) // .. _kinematic_constraints: // http://docs.ros.org/noetic/api/moveit_core/html/cpp/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132 req.group_name = "panda_arm"; - moveit_msgs::Constraints pose_goal = + moveit_msgs::msg::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); req.goal_constraints.push_back(pose_goal); @@ -168,29 +194,29 @@ int main(int argc, char** argv) /* Check that the planning was successful */ if (res.error_code_.val != res.error_code_.SUCCESS) { - ROS_ERROR("Could not compute plan successfully"); + RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); return 0; } // Visualize the result // ^^^^^^^^^^^^^^^^^^^^ - ros::Publisher display_publisher = - node_handle.advertise("/display_planned_path", 1, true); - moveit_msgs::DisplayTrajectory display_trajectory; + auto display_publisher = node->create_publisher("/display_planned_path", 1); + moveit_msgs::msg::DisplayTrajectory display_trajectory; /* Visualize the trajectory */ - ROS_INFO("Visualizing the trajectory"); - moveit_msgs::MotionPlanResponse response; + RCLCPP_INFO(LOGGER, "Visualizing the trajectory"); + moveit_msgs::msg::MotionPlanResponse response; res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); - display_publisher.publish(display_trajectory); - visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); + display_publisher->publish(display_trajectory); + // visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); /* Wait for user input */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Joint Space Goals // ^^^^^^^^^^^^^^^^^ @@ -203,7 +229,8 @@ int main(int argc, char** argv) moveit::core::RobotState goal_state(*robot_state); std::vector joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 }; goal_state.setJointGroupPositions(joint_model_group, joint_values); - moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); + moveit_msgs::msg::Constraints joint_goal = + kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); req.goal_constraints.clear(); req.goal_constraints.push_back(joint_goal); @@ -218,21 +245,22 @@ int main(int argc, char** argv) /* Check that the planning was successful */ if (res.error_code_.val != res.error_code_.SUCCESS) { - ROS_ERROR("Could not compute plan successfully"); + RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); return 0; } /* Visualize the trajectory */ - ROS_INFO("Visualizing the trajectory"); + RCLCPP_INFO(LOGGER, "Visualizing the trajectory"); res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); // Now you should see two planned trajectories in series - display_publisher.publish(display_trajectory); - visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); + display_publisher->publish(display_trajectory); + // visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); /* Wait for user input */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Using a Planning Request Adapter // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -264,22 +292,24 @@ int main(int argc, char** argv) } if (res.error_code_.val != res.error_code_.SUCCESS) { - ROS_ERROR("Could not compute plan successfully"); + RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); return 0; } /* Visualize the trajectory */ - ROS_INFO("Visualizing the trajectory"); + RCLCPP_INFO(LOGGER, "Visualizing the trajectory"); res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); /* Now you should see three planned trajectories in series*/ - display_publisher.publish(display_trajectory); - visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); + display_publisher->publish(display_trajectory); + // visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); /* Wait for user input */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo"); + prompt("Press 'Enter' to finish the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo"); */ - ROS_INFO("Done"); + RCLCPP_INFO(LOGGER, "Done"); + rclcpp::shutdown(); return 0; } diff --git a/doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst b/doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst index 670228ab40..171f9b1235 100644 --- a/doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst +++ b/doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst @@ -20,7 +20,7 @@ Running the code ---------------- Open two shells. In the first shell start RViz and wait for everything to finish loading: :: - ros2 launch moveit2_tutorials demo.launch.py + ros2 launch moveit_resources_panda_moveit_config demo.launch.py In the second shell, run the launch file for this demo: ::