Implement lite6_controller

src/drawing_controller/drawing_controller/drawing_controller.py

@@ -11,8 +11,11 @@ import math
 #from tf2_ros.transformations import quaternion_from_euler
 import lxml.etree as ET
 
-from robot_interfaces.srv import ExecuteMotion
+from rclpy.action import ActionClient
+from robot_interfaces.action import ExecuteMotion
+from robot_interfaces.msg import Motion
 import sys
+from copy import deepcopy
 
 def quaternion_from_euler(ai, aj, ak):
     ai /= 2.0
@@ -59,52 +62,68 @@ class DrawingController(Node):
         self.timer = self.create_timer(timer_period, self.timer_callback)
         self.i = 0
 
-        self.cli = self.create_client(ExecuteMotion, 'execute_path')
+        self._action_client = ActionClient(self, ExecuteMotion, 'execute_motion')
-        while not self.cli.wait_for_service(timeout_sec=1.0):
-            self.get_logger().info('service not available, waiting again...')
-        self.req = ExecuteMotion.Request()
 
-        # TODO get dimensions from svg
-
-        #print(p)
-        #print(p.position)
-        #print(p.orientation)
         xml = ET.parse(svgpath)
         svg = xml.getroot()
         self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.1, 0.5, -0.2, 0.2)
-        self.points = []
+        self.lines = []
         for child in svg:
             if (child.tag == 'line'):
-                self.points.append((float(child.get('x1')), float(child.get('y1'))))
+                p1 = (float(child.get('x1')), float(child.get('y1')))
-                self.points.append((float(child.get('x2')), float(child.get('y2'))))
+                p2 = (float(child.get('x2')), float(child.get('y2')))
+                self.lines.append((p1,p2))
 
+    def send_goal(self, motion):
+        goal_msg = ExecuteMotion.Goal()
+        goal_msg.motion = motion
 
-    def timer_callback(self):
+        self._action_client.wait_for_server()
-        next_point = self.points[self.i]
+
-        point = self.map_point(float(next_point[0]),float(next_point[1]))
+        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
+
+        self._send_goal_future.add_done_callback(self.goal_response_callback)
+
+    def goal_response_callback(self, future):
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.get_logger().info('Goal rejected :(')
+            return
+
+        self.get_logger().info('Goal accepted :)')
+
+        self._get_result_future = goal_handle.get_result_async()
+        self._get_result_future.add_done_callback(self.get_result_callback)
+
+    def get_result_callback(self, future):
+        result = future.result().result
+        self.get_logger().info('Result: {0}'.format(result))
+
+    def feedback_callback(self, feedback_msg):
+        feedback = feedback_msg.feedback
+        self.get_logger().info('Received feedback: {0}'.format(feedback))
+
+    def append_point(self, motion, point):
         p = Pose()
         p.position.x = point[0]
         p.position.y = point[1]
         p.position.z = 0.1
         q = quaternion_from_euler(0.0, math.pi, 0.0)
-        #p.orientation = q
         p.orientation.x = q[0]
         p.orientation.y = q[1]
         p.orientation.z = q[2]
         p.orientation.w = q[3]
         ps = PoseStamped()
-        ps.pose = p
+        motion.path.append(ps)
-        #print(ps)
-        #self.publisher_.publish(ps)
-        #self.get_logger().info('Publishing to /target_pose: "%s"' % p)
-        self.i = (self.i + 1) % len(self.points)
 
-        #self.req.a = a
+    def timer_callback(self):
-        #self.req.b = b
+        next_line = self.lines[self.i]
-        self.future = self.cli.call_async(self.req)
+        motion = Motion()
-        #rclpy.spin_until_future_complete(self, self.future)
+        self.append_point(motion, self.map_point(float(next_line[0][0]),float(next_line[0][1])))
-        self.get_logger().info('Got result: "%s"' % self.future.result())
+        self.append_point(motion, self.map_point(float(next_line[1][0]),float(next_line[1][1])))
-        #return self.future.result()
+        self.i = (self.i + 1) % len(self.lines)
+
+        self.send_goal(motion)
 
 
 def main(args=None):
@@ -113,13 +132,7 @@ def main(args=None):
     dc = DrawingController(sys.argv[1])
 
     rclpy.spin(dc)
-
-    # Destroy the node explicitly
-    # (optional - otherwise it will be done automatically
-    # when the garbage collector destroys the node object)
-    publisher.destroy_node()
     rclpy.shutdown()
 
-
 if __name__ == "__main__":
     main()

src/lite6_controller/CMakeLists.txt

@@ -7,9 +7,19 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
+find_package(rclcpp REQUIRED)
-# further dependencies manually.
+find_package(rclcpp_action REQUIRED)
-# find_package(<dependency> REQUIRED)
+find_package(robot_interfaces REQUIRED)
+find_package(robot_controller REQUIRED)
+find_package(moveit REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+find_package(tf2_ros REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(Eigen3 REQUIRED)
+include_directories(include  ${Boost_INCLUDE_DIR} ${EIGEN_INCLUDE_DIRS})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -22,4 +32,24 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+add_executable(lite6_controller src/lite6_controller.cpp)
+ament_target_dependencies(lite6_controller
+  "rclcpp"
+  "rclcpp_action"
+  "Eigen3"
+  "robot_controller"
+  "moveit_ros_planning_interface"
+  "robot_interfaces")
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(TARGETS
+  lite6_controller
+  DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
 ament_package()

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -0,0 +1,248 @@
+#!/usr/bin/env -S ros2 launch
+"""Launch Python example for following a target"""
+
+import os
+from ament_index_python import get_package_share_directory
+from launch.launch_description_sources import load_python_launch_file_as_module
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command, FindExecutable
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.event_handlers import OnProcessExit
+from launch.actions import OpaqueFunction
+
+
+def launch_setup(context, *args, **kwargs):
+    use_sim_time = LaunchConfiguration("use_sim_time", default=True)
+    log_level = LaunchConfiguration("log_level", default='warn')
+    rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("draw_svg"), "rviz", "ign_moveit2_examples.rviz"))
+
+    prefix = LaunchConfiguration('prefix', default='')
+    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
+    limited = LaunchConfiguration('limited', default=False)
+    effort_control = LaunchConfiguration('effort_control', default=False)
+    velocity_control = LaunchConfiguration('velocity_control', default=False)
+    add_gripper = LaunchConfiguration('add_gripper', default=False)
+    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
+    dof = LaunchConfiguration('dof', default=6)
+    robot_type = LaunchConfiguration('robot_type', default='lite')
+    ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem')
+
+    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
+    #geometry_type = LaunchConfiguration('geometry_type', default='box')
+    geometry_type = LaunchConfiguration('geometry_type', default='cylinder')
+    geometry_mass = LaunchConfiguration('geometry_mass', default=0.05)
+    geometry_height = LaunchConfiguration('geometry_height', default=0.1)
+    geometry_radius = LaunchConfiguration('geometry_radius', default=0.005)
+    geometry_length = LaunchConfiguration('geometry_length', default=0.07)
+    geometry_width = LaunchConfiguration('geometry_width', default=0.1)
+    geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
+    geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
+    geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
+    geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
+    geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
+    load_controller = LaunchConfiguration('load_controller', default=True)
+
+    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
+
+
+    ros2_control_plugin = 'gazebo_ros2_control/GazeboSystem'
+    controllers_name = 'fake_controllers'
+    moveit_controller_manager_key = 'moveit_simple_controller_manager'
+    moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager'
+
+    # robot moveit common launch
+    # xarm_moveit_config/launch/_robot_moveit_common.launch.py
+    robot_moveit_common_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])),
+        launch_arguments={
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            # 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else 'false',
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': dof,
+            'robot_type': robot_type,
+            'no_gui_ctrl': 'false',
+            'ros2_control_plugin': ros2_control_plugin,
+            'controllers_name': controllers_name,
+            'moveit_controller_manager_key': moveit_controller_manager_key,
+            'moveit_controller_manager_value': moveit_controller_manager_value,
+            'add_other_geometry': add_other_geometry,
+            'geometry_type': geometry_type,
+            'geometry_mass': geometry_mass,
+            'geometry_height': geometry_height,
+            'geometry_radius': geometry_radius,
+            'geometry_length': geometry_length,
+            'geometry_width': geometry_width,
+            'geometry_mesh_filename': geometry_mesh_filename,
+            'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
+            'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
+            'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
+            'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
+            'use_sim_time': 'true'
+        }.items(),
+    )
+
+    # robot gazebo launch
+    # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py
+    robot_gazebo_launch = IncludeLaunchDescription(
+        #PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', 'lite6_table.launch.py'])),
+        launch_arguments={
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': dof,
+            'robot_type': robot_type,
+            'ros2_control_plugin': ros2_control_plugin,
+            'load_controller': 'true',
+        }.items(),
+    )
+
+    # URDF
+    _robot_description_xml = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']
+            ),
+            #PathJoinSubstitution(
+            #    [
+            #        FindPackageShare('xarm_description'),
+            #        "urdf",
+            #        "lite6",
+            #        #"lite6.urdf.xacro",
+            #        "lite6_robot_macro.xacro",
+            #    ]
+            #),
+            " ",
+            #"name:=", "lite6", " ",
+            "prefix:=", " ",
+            "hw_ns:=", hw_ns, " ",
+            "limited:=", limited, " ",
+            "effort_control:=", effort_control, " ",
+            "velocity_control:=", velocity_control, " ",
+            "add_gripper:=", add_gripper, " ",
+            "add_vacuum_gripper:=", add_vacuum_gripper, " ",
+            "dof:=", dof, " ",
+            "robot_type:=", robot_type, " ",
+            "ros2_control_plugin:=", ros2_control_plugin, " ",
+            #"ros2_control_params:=", ros2_control_params, " ",
+
+            "add_other_geometry:=", add_other_geometry, " ",
+            "geometry_type:=", geometry_type, " ",
+            "geometry_mass:=", geometry_mass, " ",
+            "geometry_height:=", geometry_height, " ",
+            "geometry_radius:=", geometry_radius, " ",
+            "geometry_length:=", geometry_length, " ",
+            "geometry_width:=", geometry_width, " ",
+            "geometry_mesh_filename:=", geometry_mesh_filename, " ",
+            "geometry_mesh_origin_xyz:=", geometry_mesh_origin_xyz, " ",
+            "geometry_mesh_origin_rpy:=", geometry_mesh_origin_rpy, " ",
+            "geometry_mesh_tcp_xyz:=", geometry_mesh_tcp_xyz, " ",
+            "geometry_mesh_tcp_rpy:=", geometry_mesh_tcp_rpy, " ",
+
+            #"robot_ip:=", robot_ip, " ",
+            #"report_type:=", report_type, " ",
+            #"baud_checkset:=", baud_checkset, " ",
+            #"default_gripper_baud:=", default_gripper_baud, " ",
+        ]
+    )
+    # TODO fix URDF loading
+    # xacro urdf/xarm_pen.urdf.xacro prefix:= hw_ns:=xarm dof:=6 limited:=false effort_control:=false velocity_control:=false add_gripper:=false add_vacuum_gripper:=false robot_type:=lite ros2_control_plugin:=gazebo_ros2_control/GazeboSystem add_other_geometry:=false geometry_type:=cylinder geometry_mass:=0.05 geometry_height:=0.1 geometry_radius:=0.005 geometry_length:=0.07 geometry_width:=0.1 geometry_mesh_filename:= geometry_mesh_origin_xyz:="0 0 0" geometry_mesh_origin_rpy:="0 0 0" geometry_mesh_tcp_xyz:="0 0 0" geometry_mesh_tcp_rpy:="0 0 0"
+    _robot_description_xml = Command(['cat ', PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'lite6.tmp.urdf'])])
+    robot_description = {"robot_description": _robot_description_xml}
+    # SRDF
+    _robot_description_semantic_xml = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare('xarm_moveit_config'),
+                    "srdf",
+                    #"_lite6_macro.srdf.xacro",
+                    "xarm.srdf.xacro",
+                ]
+            ),
+            " ",
+            #"name:=", "lite6", " ",
+            "prefix:=", " ",
+            #"hw_ns:=", hw_ns, " ",
+            #"limited:=", limited, " ",
+            #"effort_control:=", effort_control, " ",
+            #"velocity_control:=", velocity_control, " ",
+            #"add_gripper:=", add_gripper, " ",
+            #"add_vacuum_gripper:=", add_vacuum_gripper, " ",
+            "dof:=", dof, " ",
+            "robot_type:=", robot_type, " ",
+            #"ros2_control_plugin:=", ros2_control_plugin, " ",
+            #"ros2_control_params:=", ros2_control_params, " ",
+
+            #"add_other_geometry:=", add_other_geometry, " ",
+            #"geometry_type:=", geometry_type, " ",
+            #"geometry_mass:=", geometry_mass, " ",
+            #"geometry_height:=", geometry_height, " ",
+            #"geometry_radius:=", geometry_radius, " ",
+            #"geometry_length:=", geometry_length, " ",
+            #"geometry_width:=", geometry_width, " ",
+            #"geometry_mesh_filename:=", geometry_mesh_filename, " ",
+            #"geometry_mesh_origin_xyz:=", geometry_mesh_origin_xyz, " ",
+            #"geometry_mesh_origin_rpy:=", geometry_mesh_origin_rpy, " ",
+            #"geometry_mesh_tcp_xyz:=", geometry_mesh_tcp_xyz, " ",
+            #"geometry_mesh_tcp_rpy:=", geometry_mesh_tcp_rpy, " ",
+
+            #"robot_ip:=", robot_ip, " ",
+            #"report_type:=", report_type, " ",
+            #"baud_checkset:=", baud_checkset, " ",
+            #"default_gripper_baud:=", default_gripper_baud, " ",
+        ]
+    )
+    robot_description_semantic = {
+        "robot_description_semantic": _robot_description_semantic_xml
+    }
+
+
+
+    nodes = [
+        Node(
+            package="lite6_controller",
+            executable="lite6_controller",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[
+                robot_description,
+                robot_description_semantic,
+                {"use_sim_time": use_sim_time},
+            ],
+        ),
+        Node(
+            package="draw_svg",
+            executable="drawing_surface.py",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[{"use_sim_time": use_sim_time}],
+        ),
+    ]
+
+    return [
+        robot_moveit_common_launch,
+        robot_gazebo_launch,
+    ] + nodes
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=launch_setup)
+    ])

src/lite6_controller/launch/lite6_real.launch.py

@@ -0,0 +1,361 @@
+import os
+from launch import LaunchDescription
+from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+from ament_index_python import get_package_share_directory
+from launch.launch_description_sources import load_python_launch_file_as_module
+from launch import LaunchDescription
+from launch.actions import RegisterEventHandler, EmitEvent
+from launch.event_handlers import OnProcessExit
+from launch.events import Shutdown
+
+
+def launch_setup(context, *args, **kwargs):
+    robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
+    report_type = LaunchConfiguration('report_type', default='normal')
+    prefix = LaunchConfiguration('prefix', default='')
+    hw_ns = LaunchConfiguration('hw_ns', default='ufactory')
+    limited = LaunchConfiguration('limited', default=True)
+    effort_control = LaunchConfiguration('effort_control', default=False)
+    velocity_control = LaunchConfiguration('velocity_control', default=False)
+    add_gripper = LaunchConfiguration('add_gripper', default=False)
+    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
+    dof = LaunchConfiguration('dof', default=6)
+    robot_type = LaunchConfiguration('robot_type', default='lite')
+    no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
+
+    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
+    geometry_type = LaunchConfiguration('geometry_type', default='box')
+    geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
+    geometry_height = LaunchConfiguration('geometry_height', default=0.1)
+    geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
+    geometry_length = LaunchConfiguration('geometry_length', default=0.1)
+    geometry_width = LaunchConfiguration('geometry_width', default=0.1)
+    geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
+    geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
+    geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
+    geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
+    geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
+
+    baud_checkset = LaunchConfiguration('baud_checkset', default=True)
+    default_gripper_baud = LaunchConfiguration('default_gripper_baud', default=2000000)
+
+    ros2_control_plugin = 'uf_robot_hardware/UFRobotSystemHardware'
+    controllers_name = LaunchConfiguration('controllers_name', default='controllers')
+    moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_simple_controller_manager')
+    moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_simple_controller_manager/MoveItSimpleControllerManager')
+
+    xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
+    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default=False)
+    log_level = LaunchConfiguration("log_level", default='warn')
+
+    # # robot driver launch
+    # # xarm_api/launch/_robot_driver.launch.py
+    # robot_driver_launch = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_api'), 'launch', '_robot_driver.launch.py'])),
+    #     launch_arguments={
+    #         'robot_ip': robot_ip,
+    #         'report_type': report_type,
+    #         'dof': dof,
+    #         'hw_ns': hw_ns,
+    #         'add_gripper': add_gripper,
+    #         'prefix': prefix,
+    #         'baud_checkset': baud_checkset,
+    #         'default_gripper_baud': default_gripper_baud,
+    #         'robot_type': robot_type,
+    #     }.items(),
+    # )
+
+    # robot description launch
+    # xarm_description/launch/_robot_description.launch.py
+    robot_description_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
+        launch_arguments={
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': dof,
+            'robot_type': robot_type,
+            'ros2_control_plugin': ros2_control_plugin,
+            'joint_states_remapping': PathJoinSubstitution(['/', ros_namespace, hw_ns, 'joint_states']),
+            'add_other_geometry': add_other_geometry,
+            'geometry_type': geometry_type,
+            'geometry_mass': geometry_mass,
+            'geometry_height': geometry_height,
+            'geometry_radius': geometry_radius,
+            'geometry_length': geometry_length,
+            'geometry_width': geometry_width,
+            'geometry_mesh_filename': geometry_mesh_filename,
+            'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
+            'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
+            'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
+            'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
+        }.items(),
+    )
+
+    # robot_description_parameters
+    # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
+    moveit_config_package_name = 'xarm_moveit_config'
+    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
+    get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
+    robot_description_parameters = get_xarm_robot_description_parameters(
+        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
+        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+        urdf_arguments={
+            'prefix': prefix,
+            'hw_ns': hw_ns.perform(context).strip('/'),
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': dof,
+            'robot_type': robot_type,
+            'ros2_control_plugin': ros2_control_plugin,
+            'add_other_geometry': add_other_geometry,
+            'geometry_type': geometry_type,
+            'geometry_mass': geometry_mass,
+            'geometry_height': geometry_height,
+            'geometry_radius': geometry_radius,
+            'geometry_length': geometry_length,
+            'geometry_width': geometry_width,
+            'geometry_mesh_filename': geometry_mesh_filename,
+            'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
+            'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
+            'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
+            'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
+        },
+        srdf_arguments={
+            'prefix': prefix,
+            'dof': dof,
+            'robot_type': robot_type,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'add_other_geometry': add_other_geometry,
+        },
+        arguments={
+            'context': context,
+            'xarm_type': xarm_type,
+        }
+    )
+
+    load_yaml = getattr(mod, 'load_yaml')
+    controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context)))
+    ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
+    kinematics_yaml = robot_description_parameters['robot_description_kinematics']
+    joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
+
+    if add_gripper.perform(context) in ('True', 'true'):
+        gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context)))
+        gripper_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml')
+        gripper_joint_limits_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'joint_limits.yaml')
+
+        if gripper_controllers_yaml and 'controller_names' in gripper_controllers_yaml:
+            for name in gripper_controllers_yaml['controller_names']:
+                if name in gripper_controllers_yaml:
+                    if name not in controllers_yaml['controller_names']:
+                        controllers_yaml['controller_names'].append(name)
+                    controllers_yaml[name] = gripper_controllers_yaml[name]
+        if gripper_ompl_planning_yaml:
+            ompl_planning_yaml.update(gripper_ompl_planning_yaml)
+        if joint_limits_yaml and gripper_joint_limits_yaml:
+            joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits'])
+
+    add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
+    add_prefix_to_moveit_params(
+        controllers_yaml=controllers_yaml, ompl_planning_yaml=ompl_planning_yaml,
+        kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml,
+        prefix=prefix.perform(context))
+
+    # Planning Configuration
+    ompl_planning_pipeline_config = {
+        'move_group': {
+            'planning_plugin': 'ompl_interface/OMPLPlanner',
+            'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization 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,
+        }
+    }
+    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
+
+    # Moveit controllers Configuration
+    moveit_controllers = {
+        moveit_controller_manager_key.perform(context): controllers_yaml,
+        'moveit_controller_manager': moveit_controller_manager_value.perform(context),
+    }
+
+    # Trajectory Execution Configuration
+    trajectory_execution = {
+        'moveit_manage_controllers': True,
+        'trajectory_execution.allowed_execution_duration_scaling': 1.2,
+        'trajectory_execution.allowed_goal_duration_margin': 0.5,
+        'trajectory_execution.allowed_start_tolerance': 0.01,
+        'trajectory_execution.execution_duration_monitoring': False
+    }
+
+    plan_execution = {
+        'plan_execution.record_trajectory_state_frequency': 10.0,
+    }
+
+    planning_scene_monitor_parameters = {
+        'publish_planning_scene': True,
+        'publish_geometry_updates': True,
+        'publish_state_updates': True,
+        'publish_transforms_updates': True,
+        # "planning_scene_monitor_options": {
+        #     "name": "planning_scene_monitor",
+        #     "robot_description": "robot_description",
+        #     "joint_state_topic": "/joint_states",
+        #     "attached_collision_object_topic": "/move_group/planning_scene_monitor",
+        #     "publish_planning_scene_topic": "/move_group/publish_planning_scene",
+        #     "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
+        #     "wait_for_initial_state_timeout": 10.0,
+        # },
+    }
+
+    # Start the actual move_group node/action server
+    move_group_node = Node(
+        package='moveit_ros_move_group',
+        executable='move_group',
+        output='screen',
+        parameters=[
+            robot_description_parameters,
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            plan_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            {'use_sim_time': use_sim_time},
+        ],
+    )
+
+    # rviz with moveit configuration
+    # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'config', xarm_type, 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
+    rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
+    rviz2_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', rviz_config_file],
+        parameters=[
+            robot_description_parameters,
+            ompl_planning_pipeline_config,
+            {'use_sim_time': use_sim_time},
+        ],
+        remappings=[
+            ('/tf', 'tf'),
+            ('/tf_static', 'tf_static'),
+        ]
+    )
+
+    # Static TF
+    static_tf = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='static_transform_publisher',
+        output='screen',
+        arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'link_base'],
+    )
+
+
+    # joint state publisher node
+    joint_state_publisher_node = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        output='screen',
+        parameters=[{'source_list': ['{}/joint_states'.format(hw_ns.perform(context))]}],
+        remappings=[
+            ('follow_joint_trajectory', '{}{}_traj_controller/follow_joint_trajectory'.format(prefix.perform(context), xarm_type)),
+        ],
+    )
+
+    # ros2 control launch
+    # xarm_controller/launch/_ros2_control.launch.py
+    ros2_control_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_controller'), 'launch', '_ros2_control.launch.py'])),
+        launch_arguments={
+            'robot_ip': robot_ip,
+            'report_type': report_type,
+            'baud_checkset': baud_checkset,
+            'default_gripper_baud': default_gripper_baud,
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': dof,
+            'robot_type': robot_type,
+            'ros2_control_plugin': ros2_control_plugin,
+        }.items(),
+    )
+
+    control_node = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        arguments=[
+            '{}{}_traj_controller'.format(prefix.perform(context), xarm_type),
+            '--controller-manager', '{}/controller_manager'.format(ros_namespace)
+        ],
+    )
+
+    nodes = [
+        Node(
+            package="draw_svg",
+            executable="follow",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[
+                #robot_description_parameters['robot_description'],
+                #robot_description_parameters['robot_description_semantic'],
+                #robot_description_parameters['robot_description_planning'],
+                #robot_description_parameters['robot_description_kinematics'],
+                robot_description_parameters,
+                {"use_sim_time": use_sim_time},
+            ],
+        ),
+        Node(
+            package="draw_svg",
+            executable="draw_svg.py",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[{"use_sim_time": use_sim_time}],
+        ),
+    ]
+
+    return [
+
+        RegisterEventHandler(event_handler=OnProcessExit(
+            target_action=rviz2_node,
+            on_exit=[EmitEvent(event=Shutdown())]
+        )),
+        rviz2_node,
+        static_tf,
+        move_group_node,
+
+
+        robot_description_launch,
+        joint_state_publisher_node,
+        ros2_control_launch,
+        control_node,
+        # robot_driver_launch,
+    ] + nodes
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=launch_setup)
+    ])

src/lite6_controller/package.xml

@@ -9,6 +9,22 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rclcpp</depend>
+  <!-- <depend>robot_interfaces</depend> -->
+  <!-- <depend>robot_controller</depend> -->
+  <depend>moveit</depend>
+  <depend>moveit_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
+
+  <build_depend>eigen</build_depend>
+  <build_depend>geometric_shapes</build_depend>
+  <build_depend>moveit_common</build_depend>
+  <build_depend>moveit_ros_planning</build_depend>
+
+  <depend>xarm_description</depend>
+  <depend>xarm_moveit_config</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/lite6_controller/rebuild.sh

@@ -1,8 +0,0 @@
-rm -r build/ install/ log/
-rosdep install --from-paths . --ignore-src -r -y
-colcon build
-vcs import --recursive --shallow . < lite6_controller.repos
-rosdep update
-apt-get update
-rosdep install -y -r -i
-#ros2 launch draw_svg draw_svg.launch.py

src/lite6_controller/src/lite6_controller.cpp

@@ -0,0 +1,112 @@
+#include <cstdio>
+#include <rclcpp/rclcpp.hpp>
+#include <robot_controller/robot_controller.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit_msgs/msg/collision_object.hpp>
+
+const std::string MOVE_GROUP = "lite6";
+
+//
+class Lite6Controller : public RobotController
+{
+public:
+  /// Move group interface for the robot
+  moveit::planning_interface::MoveGroupInterface move_group;
+
+  //bool moved = false;
+
+  Lite6Controller(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+    : RobotController(options),
+      move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
+  {
+    // Use upper joint velocity and acceleration limits
+    //this->move_group.setMaxAccelerationScalingFactor(1.0);
+    //this->move_group.setMaxVelocityScalingFactor(1.0);
+
+    // Subscribe to target pose
+    //target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
+
+    RCLCPP_INFO(this->get_logger(), "Initialization successful.");
+
+  }
+
+  // TODO implement time param
+  // https://moveit.picknik.ai/foxy/doc/move_group_interface/move_group_interface_tutorial.html
+  // https://groups.google.com/g/moveit-users/c/MOoFxy2exT4
+  // https://docs.ros.org/en/noetic/api/moveit_msgs/html/msg/RobotTrajectory.html
+
+  // TODO implement feedback
+  // https://answers.ros.org/question/249995/how-to-check-sate-of-plan-execution-in-moveit-during-async-execution-in-python/
+
+  /// Callback that executes path on robot
+  virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
+  {
+    RCLCPP_INFO(this->get_logger(), "Executing goal");
+    rclcpp::Rate loop_rate(20);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
+    auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
+
+    std::vector<geometry_msgs::msg::Pose> waypoints;
+
+    for (auto p : goal->motion.path)
+      waypoints.push_back(p.pose);
+
+    moveit_msgs::msg::RobotTrajectory trajectory;
+
+    // dangerous with real robot
+    // https://moveit.picknik.ai/galactic/doc/examples/move_group_interface/move_group_interface_tutorial.html
+    const double jump_threshold = 0.0;
+
+    const double eef_step = 0.01;
+    double fraction = this->move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
+    RCLCPP_INFO(this->get_logger(), "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);
+
+    this->move_group.execute(trajectory);
+
+    //waypoints.clear();
+
+    //for (int i = 1; (i <= 10) && rclcpp::ok(); ++i) {
+    //  // Check if there is a cancel request
+    //  if (goal_handle->is_canceling()) {
+    //    result->result = feedback->status;
+    //    goal_handle->canceled(result);
+    //    RCLCPP_INFO(this->get_logger(), "Goal canceled");
+    //    return;
+    //  }
+    //  // Update status
+    //  feedback->status = std::to_string(i) + "/10 complete";
+    //  // Publish feedback
+    //  goal_handle->publish_feedback(feedback);
+    //  RCLCPP_INFO(this->get_logger(), feedback->status.c_str());
+
+    //  loop_rate.sleep();
+    //}
+
+    // Check if goal is done
+    if (rclcpp::ok()) {
+      result->result = "success";
+      goal_handle->succeed(result);
+      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+    }
+  }
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting lite6_controller");
+  auto lite6 = std::make_shared<Lite6Controller>();
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(lite6);
+  executor.spin();
+
+  rclcpp::shutdown();
+  return EXIT_SUCCESS;
+}

src/robot_controller/CMakeLists.txt

@@ -1,31 +1,57 @@
 cmake_minimum_required(VERSION 3.8)
 project(robot_controller)
 
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(robot_interfaces REQUIRED)
 
-if(BUILD_TESTING)
+include_directories(include)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
 
-add_executable(robot_controller src/cpp/robot_controller.cpp)
+add_library(robot_controller src/cpp/robot_controller.cpp)
-ament_target_dependencies(robot_controller rclcpp robot_interfaces)
+add_executable(dummy_controller src/cpp/dummy_controller.cpp)
+ament_target_dependencies(robot_controller
+  "rclcpp"
+  "rclcpp_action"
+  "robot_interfaces")
+ament_target_dependencies(dummy_controller
+  "rclcpp"
+  "rclcpp_action"
+  "robot_interfaces")
 
-install(TARGETS
+ament_export_targets(robot_controller HAS_LIBRARY_TARGET)
-  robot_controller
+
+install(
+  DIRECTORY include/robot_controller
+  DESTINATION include/
+)
+install(
+  TARGETS robot_controller
+  EXPORT robot_controller
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+#if(BUILD_TESTING)
+#  find_package(ament_lint_auto REQUIRED)
+#  ament_lint_auto_find_test_dependencies()
+#endif()
+
+install(
+  TARGETS dummy_controller
   DESTINATION lib/${PROJECT_NAME})
 
+
+
 ament_package()

src/robot_controller/include/robot_controller/robot_controller.hpp

@@ -0,0 +1,85 @@
+#ifndef ROBOT_CONTROLLER_H
+#define ROBOT_CONTROLLER_H
+
+#include "robot_interfaces/action/execute_motion.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+class RobotController : public rclcpp::Node
+{
+public:
+  using ExecuteMotion = robot_interfaces::action::ExecuteMotion;
+  using GoalHandleExecuteMotion = rclcpp_action::ServerGoalHandle<ExecuteMotion>;
+
+  explicit RobotController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+private:
+  rclcpp_action::Server<ExecuteMotion>::SharedPtr action_server_;
+
+  virtual rclcpp_action::GoalResponse motion_handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const ExecuteMotion::Goal> goal);
+
+  virtual rclcpp_action::CancelResponse motion_handle_cancel(
+    const std::shared_ptr<GoalHandleExecuteMotion> goal_handle);
+
+  virtual void motion_handle_accepted(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle);
+
+  /// Callback that executes path on robot
+  virtual void executePath(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle);
+};
+
+using ExecuteMotion = robot_interfaces::action::ExecuteMotion;
+using GoalHandleExecuteMotion = rclcpp_action::ServerGoalHandle<ExecuteMotion>;
+
+RobotController::RobotController(const rclcpp::NodeOptions & options)
+: Node("robot_controller",options)
+{
+  using namespace std::placeholders;
+
+  this->action_server_ = rclcpp_action::create_server<ExecuteMotion>(
+    this,
+    "execute_motion",
+    std::bind(&RobotController::motion_handle_goal, this, _1, _2),
+    std::bind(&RobotController::motion_handle_cancel, this, _1),
+    std::bind(&RobotController::motion_handle_accepted, this, _1));
+}
+
+rclcpp_action::GoalResponse RobotController::motion_handle_goal(
+  const rclcpp_action::GoalUUID & uuid,
+  std::shared_ptr<const ExecuteMotion::Goal> goal)
+{
+  RCLCPP_INFO(this->get_logger(), "Received goal request with acceleration %f", goal->motion.acceleration);
+  (void)uuid;
+  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse RobotController::motion_handle_cancel(
+  const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
+{
+  RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+  (void)goal_handle;
+  return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void RobotController::motion_handle_accepted(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
+{
+  using namespace std::placeholders;
+  // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+  std::thread{std::bind(&RobotController::executePath, this, _1), goal_handle}.detach();
+}
+
+/// Callback that executes path on robot
+void RobotController::executePath(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
+{
+  const auto goal = goal_handle->get_goal();
+  auto feedback = std::make_shared<ExecuteMotion::Feedback>();
+  auto result = std::make_shared<ExecuteMotion::Result>();
+  std::string msg = "executePath not implemented";
+  result->result = msg;
+  feedback->status = msg;
+  RCLCPP_WARN(this->get_logger(), msg.c_str());
+}
+
+#endif

src/robot_controller/package.xml

@@ -10,6 +10,8 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend>
   <depend>robot_interfaces</depend>
 
   <test_depend>ament_lint_auto</test_depend>

src/robot_controller/src/cpp/dummy_controller.cpp

@@ -0,0 +1,67 @@
+#include <cstdio>
+#include <functional>
+#include <memory>
+#include <thread>
+
+#include "robot_controller/robot_controller.hpp"
+#include "robot_interfaces/action/execute_motion.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+// A controller for a Dummy robot. Only logs messages and serves as an example for real implementation.
+class DummyController : public RobotController
+{
+  public:
+  DummyController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : RobotController(options) {}
+
+  /// Callback that executes path on robot
+  virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
+  {
+    RCLCPP_INFO(this->get_logger(), "Executing goal");
+    rclcpp::Rate loop_rate(20);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
+    auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
+
+
+    for (int i = 1; (i <= 10) && rclcpp::ok(); ++i) {
+      // Check if there is a cancel request
+      if (goal_handle->is_canceling()) {
+        result->result = feedback->status;
+        goal_handle->canceled(result);
+        RCLCPP_INFO(this->get_logger(), "Goal canceled");
+        return;
+      }
+      // Update status
+      feedback->status = std::to_string(i) + "/10 complete";
+      // Publish feedback
+      goal_handle->publish_feedback(feedback);
+      RCLCPP_INFO(this->get_logger(), feedback->status.c_str());
+
+      loop_rate.sleep();
+    }
+
+    // Check if goal is done
+    if (rclcpp::ok()) {
+      result->result = "success";
+      goal_handle->succeed(result);
+      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+    }
+  }
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting dummy_controller");
+  auto robot = std::make_shared<DummyController>();
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(robot);
+  executor.spin();
+
+  rclcpp::shutdown();
+  return EXIT_SUCCESS;
+}

src/robot_controller/src/cpp/robot_controller.cpp

@@ -1,63 +1,15 @@
 #include <cstdio>
+#include <functional>
+#include <memory>
+#include <thread>
+
+#include "robot_controller/robot_controller.hpp"
+#include "robot_interfaces/action/execute_motion.hpp"
 #include "rclcpp/rclcpp.hpp"
-#include "robot_interfaces/srv/execute_motion.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
 
-class RobotController : public rclcpp::Node
+//#include "robot_interfaces/visibility_control.h"
-{
-public:
-  RobotController(std::string name) : Node(name)
-  {
-    this->service = this->create_service<robot_interfaces::srv::ExecuteMotion>("execute_path", std::bind(&RobotController::executePath, this, std::placeholders::_1, std::placeholders::_2));
-  }
 
-  /// Callback that executes path on robot
+//
-  virtual void executePath(const std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Request> request, std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Response> response)
+// https://docs.ros.org/en/foxy/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html
-  {
-    //request->motion->path PoseStamped[]
-    //request->motion->acceleration float64
-    //request->motion->velocity float64
-    RCLCPP_INFO(this->get_logger(), "NEW MOTION: Acceleration: ");
-    //RCLCPP_INFO(this->get_logger(), "Acceleration: " + std::to_string(request.motion.acceleration));
-    response->status = "executePath not implemented";
-    RCLCPP_WARN(this->get_logger(), "executePath not implemented");
-  }
-
-private:
-  rclcpp::Service<robot_interfaces::srv::ExecuteMotion>::SharedPtr service;
-};
-
-// A controller for a Dummy robot. Only logs messages and serves as an example for real implementation.
-class DummyController : public RobotController
-{
-  public:
-
-    DummyController(std::string name) : RobotController(name) {}
-
-    virtual void executePath(const std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Request> request, std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Response> response)
-    {
-      //request->motion->path PoseStamped[]
-      //request->motion->acceleration float64
-      //request->motion->velocity float64
-      RCLCPP_INFO(this->get_logger(), "NEW MOTION: Acceleration: ");
-      //RCLCPP_INFO(this->get_logger(), "Acceleration: " + std::to_string(request.motion.acceleration));
-      response->status = "executePath not implemented";
-      RCLCPP_WARN(this->get_logger(), "AAAAAA executePath not implemented");
-    }
-
-};
-
-
-int main(int argc, char ** argv)
-{
-  rclcpp::init(argc, argv);
-
-  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting dummy_controller");
-  auto robot = std::make_shared<DummyController>("dummy");
-
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.add_node(robot);
-  executor.spin();
-
-  rclcpp::shutdown();
-  return EXIT_SUCCESS;
-}

src/robot_interfaces/CMakeLists.txt

@@ -29,6 +29,7 @@ find_package(rclcpp REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Motion.msg"
   "srv/ExecuteMotion.srv"
+  "action/ExecuteMotion.action"
   DEPENDENCIES geometry_msgs # Add packages that above messages depend on
 )
 

src/robot_interfaces/action/ExecuteMotion.action

@@ -0,0 +1,7 @@
+# Motion action
+
+Motion motion
+---
+string result
+---
+string status

src/robot_interfaces/package.xml

@@ -13,6 +13,7 @@
   <test_depend>ament_lint_common</test_depend>
 
   <depend>geometry_msgs</depend>
+  <depend>action_msgs</depend>
 
   <build_depend>rosidl_default_generators</build_depend>