controller state broadcaster working

mars_rover/CMakeLists.txt

@@ -9,7 +9,11 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(rclpy REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -32,13 +36,14 @@ install(DIRECTORY
   models
   meshes
   worlds
+  config
   DESTINATION share/${PROJECT_NAME}
 )
 
 ament_python_install_package(${PROJECT_NAME})
 
 install(PROGRAMS
-  nodes/test_node
+  nodes/move_arm
   DESTINATION lib/${PROJECT_NAME}
 )
 

mars_rover/config/mars_rover_control.yaml

@@ -0,0 +1,21 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - arm_01_joint
+    interface_name: position
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity

mars_rover/launch/mars_rover.launch.py

@@ -1,9 +1,10 @@
 from http.server import executable
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler
 from launch.substitutions import TextSubstitution, PathJoinSubstitution, LaunchConfiguration, Command
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
+from launch.event_handlers import OnProcessExit
 import os
 from os import environ
 
@@ -60,10 +61,43 @@ def generate_launch_description():
         output='screen'
     )
 
+
+    component_state_msg = '{name: "IgnitionSystem", target_state: {id: 3, label: ""}}'
+
+    set_hardware_interface_active = ExecuteProcess(
+        cmd=['ros2', 'service', 'call', 
+            'controller_manager/set_hardware_component_state',
+            'controller_manager_msgs/srv/SetHardwareComponentState',
+            component_state_msg]
+    )
+
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    load_joint_traj_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
+             'joint_trajectory_controller'],
+        output='screen'
+    )
+
+    set_hardware_interface_active_event = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=set_hardware_interface_active,
+            on_exit=[load_joint_state_broadcaster, load_joint_traj_controller]
+        )
+    )
+
     ld.add_action(model_arg)
     ld.add_action(test_node)
     ld.add_action(start_world)
     ld.add_action(robot_state_publisher)
     ld.add_action(spawn)
+    ld.add_action(set_hardware_interface_active)
+    ld.add_action(load_joint_state_broadcaster)
+    ld.add_action(load_joint_traj_controller)
+    #ld.add_action(set_hardware_interface_active_event)
 
     return ld

mars_rover/nodes/move_arm

@@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+
+from std_msgs.msg import String, Float64
+
+class MoveArm(Node):
+
+    def __init__(self):
+        super().__init__('arm_node')
+        self.arm1_publisher_ = self.create_publisher(Float64, 'topic', 10)
+        timer_period = 0.5  # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+
+    def timer_callback(self):
+        ...
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    arm_node = MoveArm()
+
+    rclpy.spin(arm_node)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    arm_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

mars_rover/urdf/curiosity_mars_rover.xacro

@@ -25,4 +25,24 @@
 	<xacro:arm/>
 	<xacro:sensor_mast/>
 
+
+    <ros2_control name="IgnitionSystem" type="system">
+        <hardware>
+            <plugin>ign_ros2_control/IgnitionSystem</plugin>
+        </hardware>
+        <joint name="arm_01_joint">
+            <command_interface name="position">
+            </command_interface>
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+    </ros2_control>
+
+    <gazebo>
+        <plugin filename="libign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+        <robot_param>robot_description</robot_param>
+        <robot_param_node>robot_state_publisher</robot_param_node>
+        <parameters>$(find mars_rover)/config/mars_rover_control.yaml</parameters>
+        </plugin>
+    </gazebo>
 </robot>