Add a MoveIt C++ API demo

lbr_bringup/launch/move_group.launch.py

@@ -21,7 +21,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
         robot_name=model,
         package_name=f"{model}_moveit_config",
     )
-    movegroup_params = LBRMoveGroupMixin.params_move_group()
+    move_group_params = LBRMoveGroupMixin.params_move_group()
 
     # MoveGroup:
     # - requires world frame
@@ -45,7 +45,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
         LBRMoveGroupMixin.node_move_group(
             parameters=[
                 moveit_configs_builder.to_dict(),
-                movegroup_params,
+                move_group_params,
                 {"use_sim_time": LaunchConfiguration("sim")},
             ],
             namespace=robot_name,

lbr_bringup/launch/real.launch.py

@@ -88,13 +88,13 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
         robot_name=model,
         package_name=f"{model}_moveit_config",
     )
-    movegroup_params = LBRMoveGroupMixin.params_move_group()
+    move_group_params = LBRMoveGroupMixin.params_move_group()
 
     ld.add_action(
         LBRMoveGroupMixin.node_move_group(
             parameters=[
                 moveit_configs_builder.to_dict(),
-                movegroup_params,
+                move_group_params,
                 {"use_sim_time": False},
             ],
             condition=IfCondition(LaunchConfiguration("moveit")),

lbr_bringup/launch/sim.launch.py

@@ -70,13 +70,13 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
         robot_name=model,
         package_name=f"{model}_moveit_config",
     )
-    movegroup_params = LBRMoveGroupMixin.params_move_group()
+    move_group_params = LBRMoveGroupMixin.params_move_group()
 
     ld.add_action(
         LBRMoveGroupMixin.node_move_group(
             parameters=[
                 moveit_configs_builder.to_dict(),
-                movegroup_params,
+                move_group_params,
                 {"use_sim_time": True},
             ],
             condition=IfCondition(LaunchConfiguration("moveit")),

lbr_bringup/launch_mixins/lbr_bringup.py

@@ -67,7 +67,7 @@ def moveit_configs_builder(
                     f"urdf/{robot_name}/{robot_name}.xacro",
                 ),
             )
-            .planning_pipelines(default_planning_pipeline="ompl", pipelines=["ompl"])
+            .planning_pipelines(default_planning_pipeline="ompl")
         )
 
     @staticmethod

lbr_demos/doc/lbr_demos.rst

@@ -30,6 +30,7 @@ MoveIt
     :titlesonly:
 
     lbr_moveit2_py <../lbr_moveit_py/doc/lbr_moveit_py.rst>
+    lbr_moveit2_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst>
 
 Integration
 -----------

lbr_demos/lbr_moveit_cpp/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.22)
+project(lbr_moveit_cpp)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(rclcpp REQUIRED)
+
+add_executable(hello_moveit src/hello_moveit.cpp)
+
+ament_target_dependencies(hello_moveit
+  geometry_msgs  
+  moveit_ros_planning_interface
+  rclcpp
+)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(TARGETS hello_moveit
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst

@@ -0,0 +1,86 @@
+lbr_moveit_cpp
+==============
+.. note::
+
+    Also refer to the official `MoveIt <https://moveit.picknik.ai/humble/doc/tutorials/your_first_project/your_first_project.html>`_:octicon:`link-external` documentation.
+
+.. contents:: Table of Contents
+   :depth: 2
+   :local:
+   :backlinks: none
+
+MoveIt via C++ API
+------------------
+Simulation
+~~~~~~~~~~
+#. Run the robot driver:
+
+    .. code-block:: bash
+
+        ros2 launch lbr_bringup bringup.launch.py \
+            moveit:=true \
+            sim:=true \
+            model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
+
+#. Run the `hello_moveit <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_demos/lbr_moveit_cpp/src/hello_moveit.cpp>`_:octicon:`link-external` node:
+
+    .. code-block:: bash
+
+        ros2 launch lbr_moveit_cpp hello_moveit.launch.py \
+            sim:=true \
+            model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
+
+Hardware
+~~~~~~~~
+#. Client side configurations:
+
+    #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_system_parameters.yaml>`_:octicon:`link-external`
+    #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_controllers.yaml>`_:octicon:`link-external`
+
+#. Remote side configurations:
+
+    #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
+
+        .. thumbnail:: ../../doc/img/applications_lbr_server.png
+
+    #. Select
+
+        - ``FRI send period``: ``10 ms``
+        - ``IP address``: ``your configuration``
+        - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL``
+        - ``FRI client command mode``: ``POSITION``
+
+#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``.
+
+Examining the Code
+~~~~~~~~~~~~~~~~~~
+The source code for this demo is available on `GitHub <https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/humble/lbr_demos/lbr_moveit_cpp>`_:octicon:`link-external`. The demo vastly follows the official `MoveIt <https://moveit.picknik.ai/humble/doc/tutorials/your_first_project/your_first_project.html>`_:octicon:`link-external` demo.
+
+Differently, this repository puts the ``MoveGroup`` under a namespace. The ``MoveGroup`` is thus created as follows:
+
+.. code-block:: cpp
+
+    // Create MoveGroupInterface (lives inside robot_name namespace)
+    auto move_group_interface = moveit::planning_interface::MoveGroupInterface(
+        node_ptr, moveit::planning_interface::MoveGroupInterface::Options("arm", "robot_description",
+                                                                        robot_name));
+
+The ``MoveGroup`` configurations are parsed conveniently through a mixin:
+
+.. code-block:: python
+
+    from launch_mixins.lbr_bringup import LBRMoveGroupMixin
+
+    ...
+
+    model = LaunchConfiguration("model").perform(context)
+
+    # generate moveit configs
+    moveit_configs = LBRMoveGroupMixin.moveit_configs_builder(
+        robot_name=model,
+        package_name=f"{model}_moveit_config",
+    )
+
+.. note::
+
+    The MoveIt configurations might vary depending the user's configurations.

lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py

@@ -0,0 +1,46 @@
+from typing import List
+
+from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_mixins.lbr_bringup import LBRMoveGroupMixin
+from launch_mixins.lbr_description import LBRDescriptionMixin
+from launch_ros.actions import Node
+
+
+def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
+    ld = LaunchDescription()
+
+    model = LaunchConfiguration("model").perform(context)
+
+    # generate moveit configs
+    moveit_configs = LBRMoveGroupMixin.moveit_configs_builder(
+        robot_name=model,
+        package_name=f"{model}_moveit_config",
+    )
+
+    # launch demo node
+    ld.add_action(
+        Node(
+            package="lbr_moveit_cpp",
+            executable="hello_moveit",
+            parameters=[
+                moveit_configs.to_dict(),
+                {"use_sim_time": LaunchConfiguration("sim")},
+                LBRDescriptionMixin.param_robot_name(),
+            ],
+        )
+    )
+    return ld.entities
+
+
+def generate_launch_description() -> LaunchDescription:
+    ld = LaunchDescription()
+
+    ld.add_action(LBRDescriptionMixin.arg_model())
+    ld.add_action(LBRDescriptionMixin.arg_robot_name())
+    ld.add_action(LBRDescriptionMixin.arg_sim())
+
+    ld.add_action(OpaqueFunction(function=launch_setup))
+
+    return ld

lbr_demos/lbr_moveit_cpp/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lbr_moveit_cpp</name>
+  <version>2.0.0</version>
+  <description>Demo for using MoveIt C++ API.</description>
+  <maintainer email="[email protected]">mhubii</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

lbr_demos/lbr_moveit_cpp/src/hello_moveit.cpp

@@ -0,0 +1,39 @@
+#include "geometry_msgs/msg/pose.hpp"
+#include "moveit/move_group_interface/move_group_interface.h"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+
+  // Configure node
+  auto node_ptr = rclcpp::Node::make_shared("hello_moveit");
+  node_ptr->declare_parameter("robot_name", "lbr");
+  auto robot_name = node_ptr->get_parameter("robot_name").as_string();
+
+  // Create MoveGroupInterface (lives inside robot_name namespace)
+  auto move_group_interface = moveit::planning_interface::MoveGroupInterface(
+      node_ptr, moveit::planning_interface::MoveGroupInterface::Options("arm", "robot_description",
+                                                                        robot_name));
+
+  // Set a target pose
+  geometry_msgs::msg::Pose target_pose;
+  target_pose.orientation.w = 1.0;
+  target_pose.position.x = -0.4;
+  target_pose.position.y = 0.0;
+  target_pose.position.z = 0.9;
+  move_group_interface.setPoseTarget(target_pose);
+
+  // Create a plan to that target pose
+  moveit::planning_interface::MoveGroupInterface::Plan plan;
+  auto error_code = move_group_interface.plan(plan);
+
+  if (error_code == moveit::core::MoveItErrorCode::SUCCESS) {
+    // Execute the plan
+    move_group_interface.execute(plan);
+  } else {
+    RCLCPP_ERROR(node_ptr->get_logger(), "Failed to plan to target pose");
+  }
+
+  rclcpp::shutdown();
+  return 0;
+}