Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add motion planning pipeline tutorial #80

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
15 changes: 10 additions & 5 deletions doc/motion_planning_pipeline/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)
Comment on lines +3 to +4
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should it be like this?

Suggested change
target_include_directories(motion_planning_pipeline_tutorial
PUBLIC include)
target_include_directories(motion_planning_pipeline_tutorial
PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include>"))

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hey @cmelliott2 thanks for the review! I think these two points to the same include directory, but I'll have to check it out and let you know.

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}
)

This file was deleted.

Original file line number Diff line number Diff line change
@@ -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])
17 changes: 7 additions & 10 deletions doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.rst
Original file line number Diff line number Diff line change
@@ -1,8 +1,3 @@
:moveit1:

..
Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag)

Motion Planning Pipeline
==================================

Expand All @@ -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
---------------
Expand All @@ -48,4 +45,4 @@ The entire code can be seen :codedir:`here in the MoveIt GitHub project<motion_p

The Launch File
---------------
The entire launch file is :codedir:`here <motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch>` 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 <motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch.py>` 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.
106 changes: 68 additions & 38 deletions doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,8 @@

/* Author: Sachin Chitta, Mike Lautman*/

#include <pluginlib/class_loader.h>
#include <ros/ros.h>
#include <pluginlib/class_loader.hpp>
#include <rclcpp/rclcpp.hpp>

// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
Expand All @@ -44,16 +44,40 @@
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit_msgs/msg/planning_scene.hpp>
/* #include <moveit_visual_tools/moveit_visual_tools.h> This has not been ported to ros2 yet */
#include <rviz_visual_tools/rviz_visual_tools.hpp>
/* this is a standin for moveit_visual_tools visual_tools.prompt */
#include <moveit/macros/console_colors.h>

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
Expand All @@ -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 */
Expand Down Expand Up @@ -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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should be
rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "motion_planning_pipeline_tutorial", node);

and we need to subscribe to the markers in the rviz config.
image

This is the only message that RvizVisualTools prints. Would you mind adding some more for each robot move?

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();
Expand All @@ -125,15 +150,16 @@ 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
// ^^^^^^^^^
// We will now create a motion plan request for the right arm of the Panda
// 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;
Expand All @@ -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);

Expand All @@ -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<moveit_msgs::DisplayTrajectory>("/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
auto display_publisher = node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should we remove this publisher? My guess is that the planning_pipeline and this are both displaying the result which causes it to be displayed twice.
double_traj_playback

// 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
// ^^^^^^^^^^^^^^^^^
Expand All @@ -203,7 +229,8 @@ int main(int argc, char** argv)
moveit::core::RobotState goal_state(*robot_state);
std::vector<double> 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);
Expand All @@ -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
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Expand Down Expand Up @@ -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;
}
Original file line number Diff line number Diff line change
Expand Up @@ -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: ::

Expand Down