✨ Your First MoveIt C++ Project Tutorial

Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

doc/tutorials/quickstart_in_rviz/launch/demo.launch.py

@@ -91,8 +91,10 @@ def generate_launch_description():
     }
 
     planning_scene_monitor_parameters = {
-        "publish_planning_scene": True,
         "publish_geometry_updates": True,
+        "publish_planning_scene": True,
+        "publish_robot_description": True,
+        "publish_robot_description_semantic": True,
         "publish_state_updates": True,
         "publish_transforms_updates": True,
     }

doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst

@@ -232,12 +232,8 @@ Saving Your Configuration
 +++++++++++++++++++++++++
 RViz enables you to save your configuration under ``File->Save Config``. You should do this before continuing on to the next tutorials.
 
-Next Tutorials
-++++++++++++++
-* To easily control your robot using C++, check out the :doc:`Move Group C++ Interface </doc/examples/move_group_interface/move_group_interface_tutorial>`
 
-* To easily control your robot using Python, check out the :doc:`Move Group Python Interface </doc/examples/move_group_python_interface/move_group_python_interface_tutorial>`
+Next Tutorial
++++++++++++++
 
-* To create your own ``*_moveit_config`` package, check out the :doc:`Setup Assistant </doc/examples/setup_assistant/setup_assistant_tutorial>`
-
-* Save and restore robot states from a database or from disk using :doc:`warehouse_ros </doc/examples/persistent_scenes_and_states/persistent_scenes_and_states>`
+:doc:`/doc/tutorials/your_first_project/your_first_project`

doc/tutorials/tutorials.rst

@@ -13,3 +13,4 @@ In these tutorials, the Franka Emika Panda robot is used as a quick-start demo.
 
    getting_started/getting_started
    quickstart_in_rviz/quickstart_in_rviz_tutorial
+   your_first_project/your_first_project

doc/tutorials/your_first_project/your_first_project.rst

@@ -0,0 +1,276 @@
+Your First C++ MoveIt Project
+=============================
+
+This tutorial will quickly get you writing your first ROS project with MoveIt.
+
+Prerequisites
+-------------
+
+If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started </doc/tutorials/getting_started/getting_started>`.
+
+This tutorial assumes you understand the basics of ROS 2.
+To prepare yourself for this please complete the `Official ROS 2 Tutorials <https://docs.ros.org/en/{DISTRO}/Tutorials.html>`_ up until "Writing a simple publisher and Subscriber (C++)".
+
+Steps
+-----
+
+1 Create a package
+^^^^^^^^^^^^^^^^^^
+
+Open a terminal and `source your ROS 2 installation <https://docs.ros.org/en/{DISTRO}/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_ros_planning_interface 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_ros_planning_interface`` 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 <string>
+
+  #include <rclcpp/rclcpp.hpp>
+  #include <moveit/move_group_interface/move_group_interface.h>
+
+  int main(int argc, char * argv[])
+  {
+    // Initialize ROS and create the Node
+    rclcpp::init(argc, argv);
+    const auto node = std::make_shared<rclcpp::Node>(
+      "hello_moveit",
+      rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
+    );
+
+    // Create a ROS logger
+    const auto logger = rclcpp::get_logger("hello_moveit");
+
+    // Create a thread to run to spin a Executor
+    auto thread = std::thread([node]() {
+      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, **open a new terminal**, then source the workspace environment script in that new terminal so that 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 and MoveIt that we will use later.
+
+After that we have the normal call to initialize rclcpp and then we create our Node.
+
+.. code-block:: C++
+
+  const 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 Plan and Execute using MoveGroupInterface
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+After the creation of the thread add this block of code:
+
+.. code-block:: C++
+
+    // Create the MoveIt MoveGroup Interface
+    using moveit::planning_interface::MoveGroupInterface;
+    auto move_group_interface = MoveGroupInterface(node, "panda_arm");
+
+    // Set a target Pose
+    const auto target_pose = []{
+      geometry_msgs::msg::Pose msg;
+      msg.orientation.w = 1.0;
+      msg.position.x = 0.28;
+      msg.position.y = -0.2;
+      msg.position.z = 0.5;
+      return msg;
+    }();
+    move_group_interface.setPoseTarget(target_pose);
+
+    // Create a plan to that target pose
+    const auto [success, plan] = [&move_group_interface]{
+      moveit::planning_interface::MoveGroupInterface::Plan msg;
+      const auto ok = static_cast<bool>(move_group_interface.plan(msg));
+      return std::make_pair(ok, msg);
+    }();
+
+    // Execute the plan
+    if(success) {
+      move_group_interface.execute(plan);
+    } else {
+      RCLCPP_ERROR(logger, "Planing failed!");
+    }
+
+3.1 Build and Run
+~~~~~~~~~~~~~~~~~
+
+Just like before we need to build the code before we can run it.
+
+In the workspace directory ``ws_moveit2`` run this command:
+
+.. code-block:: bash
+
+  colcon build --mixin debug
+
+After this succeeds, we need to re-use the demo launch file from the previous tutorial to start RViz and the MoveGroup node.
+In a separate terminal, source the workspace and then execute this:
+
+.. code-block:: bash
+
+  ros2 launch moveit2_tutorials demo.launch.py
+
+Then in the ``Displays`` window under ``MotionPlanning/Planning Request`` uncheck the box ``Query Goal State``.
+
+.. image:: rviz_1.png
+   :width: 300px
+
+In a third terminal source the workspace and run your program.
+
+.. code-block:: bash
+
+  ros2 run hello_moveit hello_moveit
+
+This should cause the robot in RViz to move and end up in this pose:
+
+.. image:: rviz_2.png
+   :width: 300px
+
+3.2 Examine the code
+~~~~~~~~~~~~~~~~~~~~
+
+The first thing we do is create the MoveGroupInterface.
+Note that this is the only mutable object (other than the thread) that we create in this program.
+
+.. code-block:: C++
+
+    using moveit::planning_interface::MoveGroupInterface;
+    auto move_group_interface = MoveGroupInterface(node, "panda_arm");
+
+Then we set our target pose and plan.
+
+.. code-block:: C++
+
+    // Set a target Pose
+    const auto target_pose = []{
+      geometry_msgs::msg::Pose msg;
+      msg.orientation.w = 1.0;
+      msg.position.x = 0.28;
+      msg.position.y = -0.2;
+      msg.position.z = 0.5;
+      return msg;
+    }();
+    move_group_interface.setPoseTarget(target_pose);
+
+    // Create a plan to that target pose
+    const auto [success, plan] = [&move_group_interface]{
+      moveit::planning_interface::MoveGroupInterface::Plan msg;
+      const auto ok = static_cast<bool>(move_group_interface.plan(msg));
+      return std::make_pair(ok, msg);
+    }();
+
+Finally we execute our plan if planning was successful, otherwise we log an error:
+
+.. code-block:: C++
+
+    // Execute the plan
+    if(success) {
+      move_group_interface.execute(plan);
+    } else {
+      RCLCPP_ERROR(logger, "Planing failed!");
+    }
+
+Summary
+-------
+
+You created a ROS 2 package and wrote your first program using MoveIt.
+
+
+Further Reading
+---------------
+
+You may have noticed we used lambdas to be able to initialize objects as constants.
+This is known as a technique called IIEE.  You can read more about this pattern `here at C++ Stories <https://www.cppstories.com/2016/11/iife-for-complex-initialization/>`_.
+
+Next Step
+---------
+
+TODO