Skip to content

Commit c3c6511

Browse files
tylerjwabake48
tylerjw
and
abake48
authored
✨ planning around objects (#367)
Co-authored-by: Anthony Baker <[email protected]>
1 parent 641c0b8 commit c3c6511

File tree

6 files changed

+251
-4
lines changed

6 files changed

+251
-4
lines changed

‎doc/tutorials/planning_around_objects/hello_moveit.cpp‎

Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
#include <moveit/move_group_interface/move_group_interface.h>
2+
#include <moveit/planning_scene_interface/planning_scene_interface.h>
3+
#include <moveit_visual_tools/moveit_visual_tools.h>
4+
5+
#include <memory>
6+
#include <rclcpp/rclcpp.hpp>
7+
#include <thread>
8+
9+
int main(int argc, char* argv[])
10+
{
11+
// Initialize ROS and create the Node
12+
rclcpp::init(argc, argv);
13+
auto const node = std::make_shared<rclcpp::Node>(
14+
"hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
15+
16+
// Create a ROS logger
17+
auto const logger = rclcpp::get_logger("hello_moveit");
18+
19+
// We spin up a SingleThreadedExecutor for the current state monitor to get
20+
// information about the robot's state.
21+
rclcpp::executors::SingleThreadedExecutor executor;
22+
executor.add_node(node);
23+
auto spinner = std::thread([&executor]() { executor.spin(); });
24+
25+
// Create the MoveIt MoveGroup Interface
26+
using moveit::planning_interface::MoveGroupInterface;
27+
auto move_group_interface = MoveGroupInterface(node, "panda_arm");
28+
29+
// Construct and initialize MoveItVisualTools
30+
auto moveit_visual_tools =
31+
moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC,
32+
move_group_interface.getRobotModel() };
33+
moveit_visual_tools.deleteAllMarkers();
34+
moveit_visual_tools.loadRemoteControl();
35+
36+
// Create a closure for updating the text in rviz
37+
auto const draw_title = [&moveit_visual_tools](auto text) {
38+
auto const text_pose = [] {
39+
auto msg = Eigen::Isometry3d::Identity();
40+
msg.translation().z() = 1.0;
41+
return msg;
42+
}();
43+
moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
44+
};
45+
auto const prompt = [&moveit_visual_tools](auto text) { moveit_visual_tools.prompt(text); };
46+
auto const draw_trajectory_tool_path =
47+
[&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")](
48+
auto const trajectory) { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); };
49+
50+
// Set a target Pose
51+
auto const target_pose = [] {
52+
geometry_msgs::msg::Pose msg;
53+
msg.orientation.w = 1.0;
54+
msg.position.x = 0.28;
55+
msg.position.y = 0.4; // <---- This value was changed
56+
msg.position.z = 0.5;
57+
return msg;
58+
}();
59+
move_group_interface.setPoseTarget(target_pose);
60+
61+
// Create collision object for the robot to avoid
62+
auto const collision_object = [frame_id = move_group_interface.getPlanningFrame()] {
63+
moveit_msgs::msg::CollisionObject collision_object;
64+
collision_object.header.frame_id = frame_id;
65+
collision_object.id = "box1";
66+
shape_msgs::msg::SolidPrimitive primitive;
67+
68+
// Define the size of the box in meters
69+
primitive.type = primitive.BOX;
70+
primitive.dimensions.resize(3);
71+
primitive.dimensions[primitive.BOX_X] = 0.5;
72+
primitive.dimensions[primitive.BOX_Y] = 0.1;
73+
primitive.dimensions[primitive.BOX_Z] = 0.5;
74+
75+
// Define the pose of the box (relative to the frame_id)
76+
geometry_msgs::msg::Pose box_pose;
77+
box_pose.orientation.w = 1.0;
78+
box_pose.position.x = 0.2;
79+
box_pose.position.y = 0.2;
80+
box_pose.position.z = 0.25;
81+
82+
collision_object.primitives.push_back(primitive);
83+
collision_object.primitive_poses.push_back(box_pose);
84+
collision_object.operation = collision_object.ADD;
85+
86+
return collision_object;
87+
}();
88+
89+
// Add the collision object to the scene
90+
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
91+
planning_scene_interface.applyCollisionObject(collision_object);
92+
93+
// Create a plan to that target pose
94+
prompt("Press 'next' in the RvizVisualToolsGui window to plan");
95+
draw_title("Planning");
96+
moveit_visual_tools.trigger();
97+
auto const [success, plan] = [&move_group_interface] {
98+
moveit::planning_interface::MoveGroupInterface::Plan msg;
99+
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
100+
return std::make_pair(ok, msg);
101+
}();
102+
103+
// Execute the plan
104+
if (success)
105+
{
106+
draw_trajectory_tool_path(plan.trajectory_);
107+
moveit_visual_tools.trigger();
108+
prompt("Press 'next' in the RvizVisualToolsGui window to execute");
109+
draw_title("Executing");
110+
moveit_visual_tools.trigger();
111+
move_group_interface.execute(plan);
112+
}
113+
else
114+
{
115+
draw_title("Planning Failed!");
116+
moveit_visual_tools.trigger();
117+
RCLCPP_ERROR(logger, "Planing failed!");
118+
}
119+
120+
// Shutdown ROS
121+
rclcpp::shutdown();
122+
spinner.join();
123+
return 0;
124+
}

‎doc/tutorials/planning_around_objects/planning_around_object.png‎

94 KB
Loading

‎doc/tutorials/planning_around_objects/planning_around_objects.rst‎

Lines changed: 123 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,123 @@
1+
Planning Around Objects
2+
=======================
3+
4+
This tutorial will introduce you to inserting objects into the planning scene and planning around them.
5+
6+
Prerequisites
7+
-------------
8+
9+
If you haven't already done so, make sure you've completed the steps in :doc:`Visualizing in RViz </doc/tutorials/visualizing_in_rviz/visualizing_in_rviz>`.
10+
This project assumes you are starting with the ``hello_moveit`` project, where the previous tutorial left off.
11+
12+
Steps
13+
-----
14+
15+
1 Add include for Planning Scene Interface
16+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
17+
18+
At the top of your source file, add this to the list of includes:
19+
20+
.. code-block:: C++
21+
22+
#include <moveit/planning_scene_interface/planning_scene_interface.h>
23+
24+
2 Change the Target Pose
25+
^^^^^^^^^^^^^^^^^^^^^^^^
26+
27+
First, update the target pose with the following change to make the robot plan to a different location:
28+
29+
.. code-block:: C++
30+
31+
// Set a target Pose
32+
auto const target_pose = [] {
33+
geometry_msgs::msg::Pose msg;
34+
msg.orientation.w = 1.0;
35+
msg.position.x = 0.28;
36+
msg.position.y = 0.4; // <---- This value was changed
37+
msg.position.z = 0.5;
38+
return msg;
39+
}();
40+
move_group_interface.setPoseTarget(target_pose);
41+
42+
3 Create a Collision Object
43+
^^^^^^^^^^^^^^^^^^^^^^^^^^^
44+
45+
In the next block of code, we create a collision object.
46+
The first thing to notice is that it is being placed in the frame of the robot.
47+
If we had a perception system that reported the location of obstacles in our scene, then this is the sort of message it would build.
48+
Because this is just an example, we are creating it manually.
49+
One thing to notice at the end of this block of code is that we set the operation on this message to ``ADD``.
50+
This results in the object getting added to the collision scene.
51+
Place this code block between setting the target pose from the previous step and creating a plan.
52+
53+
.. code-block:: C++
54+
55+
// Create collision object for the robot to avoid
56+
auto const collision_object = [frame_id =
57+
move_group_interface.getPlanningFrame()] {
58+
moveit_msgs::msg::CollisionObject collision_object;
59+
collision_object.header.frame_id = frame_id;
60+
collision_object.id = "box1";
61+
shape_msgs::msg::SolidPrimitive primitive;
62+
63+
// Define the size of the box in meters
64+
primitive.type = primitive.BOX;
65+
primitive.dimensions.resize(3);
66+
primitive.dimensions[primitive.BOX_X] = 0.5;
67+
primitive.dimensions[primitive.BOX_Y] = 0.1;
68+
primitive.dimensions[primitive.BOX_Z] = 0.5;
69+
70+
// Define the pose of the box (relative to the frame_id)
71+
geometry_msgs::msg::Pose box_pose;
72+
box_pose.orientation.w = 1.0;
73+
box_pose.position.x = 0.2;
74+
box_pose.position.y = 0.2;
75+
box_pose.position.z = 0.25;
76+
77+
collision_object.primitives.push_back(primitive);
78+
collision_object.primitive_poses.push_back(box_pose);
79+
collision_object.operation = collision_object.ADD;
80+
81+
return collision_object;
82+
}();
83+
84+
4 Add the Object to the Planning Scene
85+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
86+
87+
Finally, we need to add this object to the collision scene.
88+
To do this, we use an object called the ``PlanningSceneInterface`` that uses ROS interfaces to communicate changes to the planning scene to ``MoveGroup``.
89+
This code block should directly follow the code block that creates the collision object.
90+
91+
.. code-block:: C++
92+
93+
// Add the collision object to the scene
94+
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
95+
planning_scene_interface.applyCollisionObject(collision_object);
96+
97+
98+
5 Run the Program and Observe the Change
99+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
100+
101+
Just as we did in the last tutorial, start RViz using the ``demo.launch.py`` script and run our program.
102+
103+
.. image:: planning_around_object.png
104+
105+
Summary
106+
-------
107+
108+
- You extended the program you wrote with MoveIt to plan around an object in the scene.
109+
- :codedir:`Here is a copy of the full hello_moveit.cpp source<tutorials/planning_around_objects/hello_moveit.cpp>`.
110+
111+
Further Reading
112+
---------------
113+
114+
- :doc:`Examples of using the Planning Scene for collision and constraint checking </doc/examples/planning_scene/planning_scene_tutorial>`.
115+
- :doc:`Examples of using the Planning Scene ROS API </doc/examples/planning_scene_ros_api/planning_scene_ros_api_tutorial>`.
116+
- :doc:`Example of visualizing collision objects </doc/examples/visualizing_collisions/visualizing_collisions_tutorial>`.
117+
- :doc:`Example of subframes used for planning with objects </doc/examples/subframes/subframes_tutorial>`.
118+
119+
Next Step
120+
---------
121+
122+
In the next tutorial :doc:`Pick and Place with MoveIt Task Constructor </doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor>`, you will be introduced to a higher layer tool designed to solve harder motion plans.
123+
In this next tutorial, you will create a program to pick and place an object.

‎doc/tutorials/tutorials.rst‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,4 +15,5 @@ In these tutorials, the Franka Emika Panda robot is used as a quick-start demo.
1515
quickstart_in_rviz/quickstart_in_rviz_tutorial
1616
your_first_project/your_first_project
1717
visualizing_in_rviz/visualizing_in_rviz
18+
planning_around_objects/planning_around_objects
1819
pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor

‎doc/tutorials/visualizing_in_rviz/visualizing_in_rviz.rst‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -200,8 +200,6 @@ Lastly, build your project again to make sure all the code additions are correct
200200
source /opt/ros/rolling/setup.bash
201201
colcon build --mixin debug
202202
203-
:codedir:`Here is a copy of the full hello_moveit.cpp source<tutorials/visualizing_in_rviz/hello_moveit.cpp>`.
204-
205203
6 Enable visualizations in RViz
206204
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
207205

@@ -283,8 +281,9 @@ Further Reading
283281

284282
- MoveItVisualTools has many more useful features for visualizing robot motions. `You can read more about it here <https://github.com/ros-planning/moveit_visual_tools/tree/ros2>`_.
285283
- There are also more examples of using ``MoveItVisualTools`` in :doc:`MoveItCpp Tutorial </doc/examples/moveit_cpp/moveitcpp_tutorial>`.
284+
- :codedir:`Here is a copy of the full hello_moveit.cpp source<tutorials/visualizing_in_rviz/hello_moveit.cpp>`.
286285

287286
Next Step
288287
---------
289288

290-
TODO
289+
In the next tutorial :doc:`Planning Around Objects </doc/tutorials/planning_around_objects/planning_around_objects>`, you will expand on the program you built here to add to the collision environment and see the robot plan with these changes.

‎doc/tutorials/your_first_project/your_first_project.rst‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -261,4 +261,4 @@ Further Reading
261261
Next Step
262262
---------
263263

264-
:doc:`/doc/tutorials/visualizing_in_rviz/visualizing_in_rviz`
264+
In the next tutorial :doc:`Visualizing in RViz </doc/tutorials/visualizing_in_rviz/visualizing_in_rviz>`, you will expand on the program you built here to create visual markers that make it easier to understand what MoveIt is doing.

0 commit comments

Comments
 (0)