-
Notifications
You must be signed in to change notification settings - Fork 186
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
✨ Your First MoveIt C++ Project Tutorial
Signed-off-by: Tyler Weaver <tylerjw@gmail.com>
- Loading branch information
Showing
3 changed files
with
151 additions
and
8 deletions.
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
147 changes: 147 additions & 0 deletions
147
doc/tutorials/your_first_project/your_first_project.rst
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,147 @@ | ||
Your First C++ MoveIt Project | ||
============================= | ||
|
||
This tutorial will quickly get you writing your first ROS project with MoveIt. | ||
|
||
Getting Started | ||
--------------- | ||
If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started </doc/tutorials/getting_started/getting_started>`. | ||
|
||
Prerequisites | ||
------------- | ||
|
||
This tutorial assumes you understand the basics of ROS 2. | ||
To prepare yourself for this please complete the Official ROS 2 tutorials up until "Writing a simple publisher and Subscriber (C++)". | ||
|
||
`Official ROS 2 Tutorials <https://docs.ros.org/en/${ROS_DISTRO}/Tutorials.html>`_ | ||
|
||
Tasks | ||
----- | ||
|
||
1 Create a package | ||
^^^^^^^^^^^^^^^^^^ | ||
|
||
Open a terminal and `source your ROS 2 installation <https://docs.ros.org/en/rolling/Tutorials/Configuring-ROS2-Environment.html>`_ so that ``ros2`` commands will work. | ||
|
||
Navigate to your ``ws_moveit2`` directory you created in the :doc:`Getting Started Tutorial </doc/tutorials/getting_started/getting_started>`. | ||
|
||
Change directory into the ``src`` directory, as that is where we put our source code. | ||
|
||
Create a new package with the ROS 2 command line tools: | ||
|
||
.. code-block:: bash | ||
ros2 pkg create \ | ||
--build-type ament_cmake \ | ||
--dependencies moveit rclcpp \ | ||
--node-name hello_moveit hello_moveit | ||
The output of this will show that it created some files in a new directory. | ||
|
||
Note that we added ``moveit`` and ``rclcpp`` as dependencies. | ||
This will create the necessary changes in the ``package.xml`` and ``CMakeLists.txt`` files so that we can depend on these two packages. | ||
|
||
Open the new source file created for you at ``ws_moveit2/src/hello_moveit/src/hello_moveit.cpp`` in your favorite editor. | ||
|
||
2 Create a ROS Node and Executor | ||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ | ||
|
||
This first bit is a bit of boilerplate but you should be used to seeing this from the ROS 2 tutorials. | ||
|
||
.. code-block:: C++ | ||
|
||
#include <memory> | ||
#include <thread> | ||
|
||
#include "rclcpp/rclcpp.hpp" | ||
|
||
int main(int argc, char * argv[]) | ||
{ | ||
// Initialize ROS and create the Node | ||
rclcpp::init(argc, argv); | ||
auto node = std::make_shared<rclcpp::Node>( | ||
"hello_moveit", | ||
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true) | ||
); | ||
|
||
// Create a thread to run to spin a Executor | ||
auto thread = std::thread([=]() { | ||
rclcpp::executors::SingleThreadedExecutor executor; | ||
executor.add_node(node); | ||
executor.spin(); | ||
}); | ||
|
||
// Next step goes here | ||
|
||
// Shutdown ROS and our thread | ||
rclcpp::shutdown(); | ||
thread.join(); | ||
return 0; | ||
} | ||
|
||
2.1 Build and Run | ||
^^^^^^^^^^^^^^^^^ | ||
|
||
We will build and run the program to see that everything is right before we move on. | ||
|
||
Change directory back to the workspace directory ``ws_moveit2`` and run this command: | ||
|
||
.. code-block:: bash | ||
colcon build --mixin debug | ||
After this succeeds we need to source the workspace install before we can run our program. | ||
|
||
.. code-block:: bash | ||
source install/setup.bash | ||
Run your program and see the output. | ||
|
||
.. code-block:: bash | ||
ros2 run hello_moveit hello_moveit | ||
You will probably see this error: | ||
|
||
.. code-block:: | ||
terminate called after throwing an instance of 'rclcpp::exceptions::RCLError' | ||
what(): the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at /tmp/binarydeb/ros-rolling-rcl-4.0.0/src/rcl/guard_condition.c:67 | ||
[ros2run]: Aborted | ||
This is because ``rclcpp::shutdown()`` was called before the Executor finished initializing. | ||
We'll fix that in the next section by adding some code that does something after creating the thread. | ||
|
||
2.2 Examine the code | ||
^^^^^^^^^^^^^^^^^^^^ | ||
|
||
The headers included at the top are just some standard C++ headers and the header for ROS. | ||
|
||
After that we have the normal call to initialize rclcpp and then we create our Node. | ||
|
||
.. code-block:: C++ | ||
|
||
auto node = std::make_shared<rclcpp::Node>( | ||
"hello_moveit", | ||
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true) | ||
); | ||
|
||
The first argument is the string that ROS will use to make a unique node. | ||
The second is needed for MoveIt because of how we use ROS Parameters. | ||
|
||
Before we can start use our ROS Node we have to give it to a spinning executor. | ||
This is what will enable ROS to call callbacks to update our Robot State among other things. | ||
|
||
.. code-block:: C++ | ||
|
||
auto thread = std::thread([=]() { | ||
rclcpp::executors::SingleThreadedExecutor executor; | ||
executor.add_node(node); | ||
executor.spin(); | ||
}); | ||
|
||
Lastly we have the calls to shutdown ROS and cleanup our thread. | ||
|
||
3 Create the MoveGroupInterface | ||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ |