added wheel velocity and mast position control

mars_rover/CMakeLists.txt

@@ -44,6 +44,8 @@ ament_python_install_package(${PROJECT_NAME})
 
 install(PROGRAMS
   nodes/move_arm
+  nodes/move_mast
+  nodes/move_wheel
   DESTINATION lib/${PROJECT_NAME}
 )
 

mars_rover/config/mars_rover_control.yaml

@@ -2,14 +2,20 @@ controller_manager:
   ros__parameters:
     update_rate: 100
 
-    joint_trajectory_controller:
+    arm_joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    mast_joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    wheel_velocity_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 
-joint_trajectory_controller:
+arm_joint_trajectory_controller:
   ros__parameters:
     joints:
       - arm_01_joint
@@ -20,6 +26,35 @@ joint_trajectory_controller:
     interface_name: position
     command_interfaces:
       - position
+    state_interfaces:
+      - position
+      - velocity
+
+mast_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - mast_p_joint
+      - mast_02_joint
+      - mast_cameras_joint
+    interface_name: position
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+
+wheel_velocity_controller:
+  ros__parameters:
+    joints:
+      - front_wheel_L_joint
+      - middle_wheel_L_joint
+      - back_wheel_L_joint
+      - front_wheel_R_joint
+      - middle_wheel_R_joint
+      - back_wheel_R_joint
+    interface_name: velocity
+    command_interfaces:
+      - velocity
     state_interfaces:
       - position
       - velocity

mars_rover/launch/mars_rover.launch.py

@@ -4,12 +4,12 @@
 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
+from launch.event_handlers import OnProcessExit, OnExecutionComplete
 import os
 from os import environ
 
 def generate_launch_description():
-    ld = LaunchDescription()
+    # ld = LaunchDescription()
 
     env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH':
            ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''),
@@ -63,8 +63,11 @@ def generate_launch_description():
     )
 
 
+    ## Control Components
+
     component_state_msg = '{name: "IgnitionSystem", target_state: {id: 3, label: ""}}'
 
+    ## a hack to resolve current bug
     set_hardware_interface_active = ExecuteProcess(
         cmd=['ros2', 'service', 'call', 
             'controller_manager/set_hardware_component_state',
@@ -78,27 +81,45 @@ def generate_launch_description():
         output='screen'
     )
 
-    load_joint_traj_controller = ExecuteProcess(
+    load_arm_joint_traj_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
+             'arm_joint_trajectory_controller'],
+        output='screen'
+    )
+
+    load_mast_joint_traj_controller = ExecuteProcess(
         cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
-             'joint_trajectory_controller'],
+             'mast_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]
-        )
+    load_wheel_joint_traj_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
+             'wheel_velocity_controller'],
+        output='screen'
     )
 
-    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
+
+
+
+    return LaunchDescription([
+        model_arg,
+        start_world,
+        robot_state_publisher,
+        spawn,
+        RegisterEventHandler(
+            OnProcessExit(
+                target_action=spawn,
+                on_exit=[set_hardware_interface_active],
+            )
+        ),
+        RegisterEventHandler(
+            OnProcessExit(
+                target_action=set_hardware_interface_active,
+                on_exit=[load_joint_state_broadcaster,
+                        load_arm_joint_traj_controller,
+                        load_mast_joint_traj_controller,
+                        load_wheel_joint_traj_controller],
+            )
+        ),
+    ])

mars_rover/nodes/move_arm

@@ -11,7 +11,7 @@ class MoveArm(Node):
 
     def __init__(self):
         super().__init__('arm_node')
-        self.arm1_publisher_ = self.create_publisher(JointTrajectory, '/joint_trajectory_controller/joint_trajectory', 10)
+        self.arm_publisher_ = self.create_publisher(JointTrajectory, '/arm_joint_trajectory_controller/joint_trajectory', 10)
         timer_period = 0.5  # seconds
         self.timer = self.create_timer(timer_period, self.timer_callback)
 
@@ -25,7 +25,7 @@ class MoveArm(Node):
         point.time_from_start = Duration(sec=1)
 
         traj.points.append(point)
-        self.arm1_publisher_.publish(traj)
+        self.arm_publisher_.publish(traj)
 
 
 def main(args=None):

mars_rover/nodes/move_mast

@@ -0,0 +1,46 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from builtin_interfaces.msg import Duration
+
+from std_msgs.msg import String, Float64
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+class MastArm(Node):
+
+    def __init__(self):
+        super().__init__('mast_node')
+        self.mast_publisher_ = self.create_publisher(JointTrajectory, '/mast_joint_trajectory_controller/joint_trajectory', 10)
+        timer_period = 0.5  # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+
+    def timer_callback(self):
+        traj = JointTrajectory()
+        traj.joint_names = ["mast_p_joint", "mast_02_joint", "mast_cameras_joint"]
+
+        point = JointTrajectoryPoint()
+        point.positions = [0.0,0.0,0.0]
+        point.time_from_start = Duration(sec=1)
+
+        traj.points.append(point)
+        self.mast_publisher_.publish(traj)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    mast_node = MastArm()
+
+    rclpy.spin(mast_node)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    mast_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

mars_rover/nodes/move_wheel

@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from builtin_interfaces.msg import Duration
+
+from std_msgs.msg import String, Float64
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+class MoveWheel(Node):
+
+    def __init__(self):
+        super().__init__('wheel_node')
+        self.wheel_publisher_ = self.create_publisher(JointTrajectory, '/wheel_velocity_controller/joint_trajectory', 10)
+        timer_period = 0.5  # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+
+    def timer_callback(self):
+        traj = JointTrajectory()
+        traj.joint_names = ["front_wheel_L_joint",
+                            "middle_wheel_L_joint",
+                            "back_wheel_L_joint",
+                            "front_wheel_R_joint",
+                            "middle_wheel_R_joint",
+                            "back_wheel_R_joint"]
+
+        point = JointTrajectoryPoint()
+        point.positions = [0.0,0.0,0.0,0.0,0.0,0.0]
+        point.time_from_start = Duration(sec=1)
+
+        traj.points.append(point)
+        self.wheel_publisher_.publish(traj)
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    wheel_node = MoveWheel()
+
+    rclpy.spin(wheel_node)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    wheel_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

mars_rover/urdf/curiosity_mars_rover.xacro

@@ -31,32 +31,73 @@
             <plugin>ign_ros2_control/IgnitionSystem</plugin>
         </hardware>
         <joint name="arm_01_joint">
-            <command_interface name="position">
-            </command_interface>
+            <command_interface name="position" />
             <state_interface name="position" />
             <state_interface name="velocity" />
         </joint>
         <joint name="arm_02_joint">
-            <command_interface name="position">
-            </command_interface>
+            <command_interface name="position" />
             <state_interface name="position" />
             <state_interface name="velocity" />
         </joint>
         <joint name="arm_03_joint">
-            <command_interface name="position">
-            </command_interface>
+            <command_interface name="position" />
             <state_interface name="position" />
             <state_interface name="velocity" />
         </joint>
         <joint name="arm_04_joint">
-            <command_interface name="position">
-            </command_interface>
+            <command_interface name="position" />
+
             <state_interface name="position" />
             <state_interface name="velocity" />
         </joint>
         <joint name="arm_tools_joint">
-            <command_interface name="position">
-            </command_interface>
+            <command_interface name="position" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="mast_p_joint">
+            <command_interface name="position" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="mast_02_joint">
+            <command_interface name="position" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="mast_cameras_joint">
+            <command_interface name="position" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="front_wheel_L_joint">
+            <command_interface name="velocity" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="middle_wheel_L_joint">
+            <command_interface name="velocity" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="back_wheel_L_joint">
+            <command_interface name="velocity" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="front_wheel_R_joint">
+            <command_interface name="velocity" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="middle_wheel_R_joint">
+            <command_interface name="velocity" />
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="back_wheel_R_joint">
+            <command_interface name="velocity" />
             <state_interface name="position" />
             <state_interface name="velocity" />
         </joint>