diff --git a/CMakeLists.txt b/CMakeLists.txt index 1547aa73be..349f3b2ffd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(moveit_ros_perception REQUIRED) find_package(moveit_servo REQUIRED) find_package(interactive_markers REQUIRED) find_package(rviz_visual_tools REQUIRED) -#find_package(moveit_visual_tools REQUIRED) +find_package(moveit_visual_tools REQUIRED) find_package(geometric_shapes REQUIRED) #find_package(pcl_ros REQUIRED) #find_package(pcl_conversions REQUIRED) @@ -40,7 +40,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_ros moveit_core rviz_visual_tools - # moveit_visual_tools + moveit_visual_tools moveit_ros_planning_interface interactive_markers tf2_geometry_msgs @@ -57,8 +57,8 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) # add_subdirectory(doc/controller_configuration) # 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_api) +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_api/CMakeLists.txt b/doc/motion_planning_api/CMakeLists.txt index 2a87b0f509..13414c315c 100644 --- a/doc/motion_planning_api/CMakeLists.txt +++ b/doc/motion_planning_api/CMakeLists.txt @@ -1,8 +1,13 @@ add_executable(motion_planning_api_tutorial src/motion_planning_api_tutorial.cpp) -target_link_libraries(motion_planning_api_tutorial - ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS motion_planning_api_tutorial DESTINATION - ${CATKIN_PACKAGE_BIN_DESTINATION}) +target_include_directories(motion_planning_api_tutorial + PUBLIC include) +ament_target_dependencies(motion_planning_api_tutorial + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(TARGETS motion_planning_api_tutorial + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/motion_planning_api/launch/motion_planning_api_tutorial.launch b/doc/motion_planning_api/launch/motion_planning_api_tutorial.launch deleted file mode 100644 index 05869d8bcf..0000000000 --- a/doc/motion_planning_api/launch/motion_planning_api_tutorial.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/doc/motion_planning_api/launch/motion_planning_api_tutorial.launch.py b/doc/motion_planning_api/launch/motion_planning_api_tutorial.launch.py new file mode 100644 index 0000000000..9097f3db29 --- /dev/null +++ b/doc/motion_planning_api/launch/motion_planning_api_tutorial.launch.py @@ -0,0 +1,73 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +import xacro + + +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(): + + robot_description_config = load_file( + "moveit_resources_panda_description", "urdf/panda.urdf" + ) + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + ompl_planning_pipeline_config = { + "ompl": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization 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, + } + } + ompl_planning_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) + + # Start the actual move_group node + motion_planning_node = Node( + package="moveit2_tutorials", + executable="motion_planning_api_tutorial", + prefix="xterm -e", + output="screen", + parameters=[ + {"robot_description": robot_description_config}, + {"robot_description_semantic": robot_description_semantic_config}, + {"planning_plugin": "ompl_interface/OMPLPlanner"}, + ompl_planning_pipeline_config, + kinematics_yaml, + ], + ) + + return LaunchDescription([motion_planning_node]) diff --git a/doc/motion_planning_api/motion_planning_api_tutorial.rst b/doc/motion_planning_api/motion_planning_api_tutorial.rst index 8a9ac4f93c..d9051b68b4 100644 --- a/doc/motion_planning_api/motion_planning_api_tutorial.rst +++ b/doc/motion_planning_api/motion_planning_api_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 API ================================== .. image:: motion_planning_api_tutorial_robot_move_arm_1st.png @@ -16,20 +11,20 @@ Getting Started --------------- If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_. +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: :: + + sudo apt-get install -y xterm + Running the Demo ---------------- 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_api_tutorial.launch - -**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>`_. - -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. - + ros2 launch moveit2_tutorials motion_planning_api_tutorial.launch.py Expected Output --------------- diff --git a/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp b/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp index 83d93168d6..85ea9ce3e4 100644 --- a/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp +++ b/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp @@ -34,27 +34,40 @@ /* Author: Sachin Chitta, Michael Lautman */ -#include -#include +#include // MoveIt #include #include #include #include -#include -#include +#include +#include #include -#include +#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()) + ; +} int main(int argc, char** argv) { - const std::string node_name = "motion_planning_tutorial"; - ros::init(argc, argv, node_name); - ros::AsyncSpinner spinner(1); - spinner.start(); - ros::NodeHandle node_handle("~"); + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto motion_planning_node = rclcpp::Node::make_shared("motion_planning_api_tutorial", node_options); + + const rclcpp::Logger& LOGGER = motion_planning_node->get_logger(); + + // We spin up a SingleThreadedExecutor for the current state monitor to get information + // about the robot's state. + auto executor = std::make_shared(); + executor->add_node(motion_planning_node); + std::thread([=]() { executor->spin(); }).detach(); // BEGIN_TUTORIAL // Start @@ -71,7 +84,7 @@ int main(int argc, char** argv) // .. _RobotModelLoader: // http://docs.ros.org/noetic/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html const std::string PLANNING_GROUP = "panda_arm"; - robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); + robot_model_loader::RobotModelLoader robot_model_loader(motion_planning_node); const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel(); /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/ moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model)); @@ -86,39 +99,44 @@ int main(int argc, char** argv) // We will now construct a loader to load a planner, by name. // Note that we are using the ROS pluginlib library here. - boost::scoped_ptr> planner_plugin_loader; + typedef pluginlib::ClassLoader PlannerManagerLoader; + std::unique_ptr planner_plugin_loader; planning_interface::PlannerManagerPtr planner_instance; std::string planner_plugin_name; // We will get the name of planning plugin we want to load // from the ROS parameter server, and then load the planner // making sure to catch all exceptions. - if (!node_handle.getParam("planning_plugin", planner_plugin_name)) - ROS_FATAL_STREAM("Could not find planner plugin name"); + if (!motion_planning_node->get_parameter("planning_plugin", planner_plugin_name)) + RCLCPP_FATAL_STREAM(LOGGER, "Could not find planner plugin name"); try { - planner_plugin_loader.reset(new pluginlib::ClassLoader( - "moveit_core", "planning_interface::PlannerManager")); + planner_plugin_loader = std::make_unique("moveit_core", "planning_interface::PlannerManager"); } catch (pluginlib::PluginlibException& ex) { - ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); + RCLCPP_FATAL_STREAM(LOGGER, "Exception while creating planning plugin loader " << ex.what()); } try { planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); - if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) - ROS_FATAL_STREAM("Could not initialize planner instance"); - ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'"); + if (!planner_instance->initialize(robot_model, motion_planning_node, motion_planning_node->get_namespace())) + { + RCLCPP_FATAL_STREAM(LOGGER, "Could not initialize planner instance"); + } + RCLCPP_INFO_STREAM(LOGGER, "Using planning interface '" << planner_instance->getDescription() << "'"); } catch (pluginlib::PluginlibException& ex) { const std::vector& classes = planner_plugin_loader->getDeclaredClasses(); std::stringstream ss; for (const auto& cls : classes) + { ss << cls << " "; - ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl - << "Available plugins: " << ss.str()); + } + RCLCPP_ERROR_STREAM(LOGGER, "Exception while loading planner '" << planner_plugin_name << "': " << ex.what() + << std::endl + << "Available plugins: " << ss.str()); } // Visualization @@ -126,15 +144,10 @@ int main(int argc, char** argv) // 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(motion_planning_node, "panda_link0"); visual_tools.loadRobotStatePub("/display_robot_state"); visual_tools.enableBatchPublishing(); visual_tools.deleteAllMarkers(); // clear all old markers - visual_tools.trigger(); - - /* 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(); /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/ Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); @@ -144,8 +157,7 @@ int main(int argc, char** argv) /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */ 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"); // Pose Goal // ^^^^^^^^^ @@ -155,7 +167,7 @@ int main(int argc, char** argv) visual_tools.trigger(); 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.4; @@ -174,7 +186,7 @@ int main(int argc, char** argv) // // .. _kinematic_constraints: // http://docs.ros.org/noetic/api/moveit_core/html/cpp/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132 - moveit_msgs::Constraints pose_goal = + moveit_msgs::msg::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); req.group_name = PLANNING_GROUP; @@ -188,25 +200,25 @@ int main(int argc, char** argv) context->solve(res); 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 = + motion_planning_node->create_publisher("/display_planned_path", 1); + moveit_msgs::msg::DisplayTrajectory display_trajectory; /* Visualize the trajectory */ - moveit_msgs::MotionPlanResponse response; + moveit_msgs::msg::MotionPlanResponse response; res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); - display_publisher.publish(display_trajectory); + display_publisher->publish(display_trajectory); /* Set the state in the planning scene to the final state of the last plan */ robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); @@ -219,7 +231,7 @@ 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 continue the demo"); + prompt("Press 'Enter' to continue the demo"); // Joint Space Goals // ^^^^^^^^^^^^^^^^^ @@ -227,7 +239,8 @@ int main(int argc, char** argv) moveit::core::RobotState goal_state(robot_model); 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); @@ -239,7 +252,7 @@ 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 */ @@ -249,7 +262,7 @@ int main(int argc, char** argv) /* Now you should see two planned trajectories in series*/ visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); - display_publisher.publish(display_trajectory); + display_publisher->publish(display_trajectory); /* We will add more goals. But first, set the state in the planning scene to the final state of the last plan */ @@ -263,7 +276,7 @@ int main(int argc, char** argv) 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"); /* Now, we go back to the first goal to prepare for orientation constrained planning */ req.goal_constraints.clear(); @@ -275,7 +288,7 @@ int main(int argc, char** argv) display_trajectory.trajectory.push_back(response.trajectory); visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); - display_publisher.publish(display_trajectory); + display_publisher->publish(display_trajectory); /* Set the state in the planning scene to the final state of the last plan */ robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); @@ -286,7 +299,7 @@ int main(int argc, char** argv) 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"); // Adding Path Constraints // ^^^^^^^^^^^^^^^^^^^^^^^ @@ -297,7 +310,7 @@ int main(int argc, char** argv) pose.pose.position.y = -0.25; pose.pose.position.z = 0.65; pose.pose.orientation.w = 1.0; - moveit_msgs::Constraints pose_goal_2 = + moveit_msgs::msg::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); /* Now, let's try to move to this new pose goal*/ @@ -306,7 +319,7 @@ int main(int argc, char** argv) /* But, let's impose a path constraint on the motion. Here, we are asking for the end-effector to stay level*/ - geometry_msgs::QuaternionStamped quaternion; + geometry_msgs::msg::QuaternionStamped quaternion; quaternion.header.frame_id = "panda_link0"; quaternion.quaternion.w = 1.0; req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion); @@ -331,7 +344,7 @@ int main(int argc, char** argv) display_trajectory.trajectory.push_back(response.trajectory); visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); - display_publisher.publish(display_trajectory); + display_publisher->publish(display_trajectory); /* Set the state in the planning scene to the final state of the last plan */ robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); @@ -344,9 +357,9 @@ int main(int argc, char** argv) visual_tools.trigger(); // END_TUTORIAL - /* Wait for user input */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to exit the demo"); + prompt("Press 'Enter' to exit the demo"); planner_instance.reset(); + rclcpp::shutdown(); return 0; } diff --git a/doc/motion_planning_pipeline/CMakeLists.txt b/doc/motion_planning_pipeline/CMakeLists.txt index 8dc42ca3e3..7db4b101e6 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..69481cbbb5 --- /dev/null +++ b/doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch.py @@ -0,0 +1,70 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +import xacro + + +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(): + + robot_description_config = load_file( + "moveit_resources_panda_description", "urdf/panda.urdf" + ) + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + ompl_planning_pipeline_config = { + "ompl": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization 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, + } + } + ompl_planning_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) + + # Start the actual node + motion_planning_node = Node( + package="moveit2_tutorials", + executable="motion_planning_pipeline_tutorial", + prefix="xterm -e", + output="screen", + parameters=[ + {"robot_description": robot_description_config}, + {"robot_description_semantic": robot_description_semantic_config}, + {"planning_plugin": "ompl_interface/OMPLPlanner"}, + ompl_planning_pipeline_config, + kinematics_yaml, + ], + ) diff --git a/doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.rst b/doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.rst index 99ad4a6d54..f36e2da61f 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,11 +17,11 @@ 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:** 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>`_. 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..23f5cac4da 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,7 @@ /* Author: Sachin Chitta, Mike Lautman*/ -#include -#include +#include // MoveIt #include @@ -44,16 +43,18 @@ #include #include #include -#include -#include +#include +#include #include 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; + node_options.automatically_declare_parameters_from_overrides(true); + auto motion_planning_node = rclcpp::Node::make_shared("motion_planning_pipeline_tutorial", node_options); + + const rclcpp::Logger& LOGGER = motion_planning_node->get_logger(); // BEGIN_TUTORIAL // Start @@ -66,15 +67,13 @@ 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")); + auto robot_model_loader = std::make_shared(motion_planning_node); // 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)); + auto psm = std::make_shared(motion_planning_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 */ @@ -92,8 +91,8 @@ int main(int argc, char** argv) /* We can get the most up to date robot state from the PlanningSceneMonitor by locking the internal planning scene for reading. This lock ensures that the underlying scene isn't updated while we are reading it's state. RobotState's are useful for computing the forward and inverse kinematics of the robot among many other uses */ - moveit::core::RobotStatePtr robot_state( - new moveit::core::RobotState(planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentState())); + auto robot_state = + std::make_shared(planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentState()); /* Create a JointModelGroup to keep track of the current robot pose and planning group. The Joint Model group is useful for dealing with one set of joints at a time such as a left arm or a end effector */ @@ -101,20 +100,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")); + auto planning_pipeline = std::make_shared(robot_model, motion_planning_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(motion_planning_node, "panda_link0"); 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(); + // FIXME 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(); @@ -133,7 +132,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 +152,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,24 +167,24 @@ 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 = + motion_planning_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); + display_publisher->publish(display_trajectory); visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); @@ -203,7 +202,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,16 +218,16 @@ 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); + display_publisher->publish(display_trajectory); visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); @@ -264,22 +264,23 @@ 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); + 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"); - ROS_INFO("Done"); + RCLCPP_INFO(LOGGER, "Done"); + rclcpp::shutdown(); return 0; } diff --git a/doc/move_group_interface/move_group_interface_tutorial.rst b/doc/move_group_interface/move_group_interface_tutorial.rst index 6102d503a6..d0e4a58428 100644 --- a/doc/move_group_interface/move_group_interface_tutorial.rst +++ b/doc/move_group_interface/move_group_interface_tutorial.rst @@ -12,7 +12,7 @@ Getting Started --------------- If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_. -**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. +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: :: sudo apt-get install -y xterm @@ -27,11 +27,6 @@ In the second shell, run the launch file: :: ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py -**Note:** RvizVisualToolsGui panel has not been ported to ROS2 yet -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>`_. - -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 --------------- See the `YouTube video `_ at the top of this tutorial for expected output. In RViz, we should be able to see the following: diff --git a/moveit2_tutorials.repos b/moveit2_tutorials.repos index 50f767f1db..cd616d7709 100644 --- a/moveit2_tutorials.repos +++ b/moveit2_tutorials.repos @@ -7,3 +7,11 @@ repositories: type: git url: https://github.com/ros-planning/moveit_resources version: ros2 + moveit_visual_tools: + type: git + url: https://github.com/ros-planning/moveit_visual_tools + version: ros2 + graph_msgs: + type: git + url: https://github.com/PickNikRobotics/graph_msgs + version: ros2 diff --git a/package.xml b/package.xml index 5891fc6e3a..4c95145cbc 100644 --- a/package.xml +++ b/package.xml @@ -24,7 +24,7 @@ moveit_ros_perception moveit_ros_planning_interface moveit_servo - + moveit_visual_tools pluginlib