-
Notifications
You must be signed in to change notification settings - Fork 198
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
vatanaksoytezer
wants to merge
2
commits into
moveit:main
Choose a base branch
from
vatanaksoytezer:feature/motion_planning_pipeline
base: main
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Open
Changes from all commits
Commits
Show all changes
2 commits
Select commit
Hold shift + click to select a range
File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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) | ||
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} | ||
) |
14 changes: 0 additions & 14 deletions
14
doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch
This file was deleted.
Oops, something went wrong.
66 changes: 66 additions & 0 deletions
66
doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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]) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -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> | ||
|
@@ -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 | ||
|
@@ -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); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. |
||
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,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; | ||
|
@@ -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<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); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. |
||
// 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<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); | ||
|
@@ -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; | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.