Fix canadarm's harcoded paths / update control interface / use xacro …

…file (#23)

* Fix paths

* Fix controller type

* Set parameters for xyz/rpy for arm in launch file

* Added joint traj controller and node for demo

canadarm/CMakeLists.txt

@@ -43,6 +43,7 @@ install(DIRECTORY
 
 install(PROGRAMS
   nodes/move_joint_server
+  nodes/move_arm
   DESTINATION lib/${PROJECT_NAME}
 )
 

canadarm/README.md

@@ -3,8 +3,26 @@
 
 (This is a temp instruction before the reorg)
 
-Build and source the ```canadarm``` package
+Build and source the ```canadarm``` package:
 
 ```
 ros2 launch canadarm canadarm.launch.py
-```
+```
+
+To move the arm to an outstretched pose:
+
+```
+ros2 service call /open_arm std_srvs/srv/Empty
+```
+
+To move the arm to its default (close) pose of all zeros:
+
+```
+ros2 service call /close_arm std_srvs/srv/Empty
+```
+
+To move the arm to a random pose:
+
+```
+ros2 service call /random_arm std_srvs/srv/Empty
+```

canadarm/config/canadarm_control.yaml

@@ -5,10 +5,30 @@ controller_manager:
     canadarm_joint_controller:
       type: effort_controllers/JointGroupPositionController
 
+    canadarm_joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 
+canadarm_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - Base_Joint
+      - Shoulder_Roll
+      - Shoulder_Yaw
+      - Elbow_Pitch
+      - Wrist_Pitch
+      - Wrist_Yaw
+      - Wrist_Roll
+    interface_name: position
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+
 canadarm_joint_controller:
   ros__parameters:
     joints:
@@ -53,4 +73,4 @@ canadarm_joint_controller:
       Wrist_Roll:
         p: 50.0
         i: 10.0
-        d: 200.0
+        d: 200.0

canadarm/launch/canadarm.launch.py

@@ -23,23 +23,30 @@ def generate_launch_description():
            ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''),
                      environ.get('LD_LIBRARY_PATH', default='')]),
            'IGN_GAZEBO_RESOURCE_PATH':
-           ':'.join([canadarm_demos_path])}
+           ':'.join([environ.get('IGN_GAZEBO_RESOURCE_PATH', default=''), canadarm_demos_path])}
 
 
-    urdf_model_path = os.path.join(simulation_models_path, 'models', 'canadarm', 'urdf', 'SSRMS_Canadarm2.urdf')
+    urdf_model_path = os.path.join(simulation_models_path, 'models', 'canadarm', 'urdf', 'SSRMS_Canadarm2.urdf.xacro')
     leo_model = os.path.join(canadarm_demos_path, 'worlds', 'simple.world')
 
 
-    doc = xacro.process_file(urdf_model_path)
+    doc = xacro.process_file(urdf_model_path, mappings={'xyz' : '1.0 0.0 1.5', 'rpy': '3.1416 0.0 0.0'})
     robot_description = {'robot_description': doc.toxml()}
 
 
-    run_node = Node(
+    #run_node = Node(
+    #    package="canadarm",
+    #    executable="move_joint_server",
+    #    output='screen'
+    #)
+
+    run_move_arm = Node(
         package="canadarm",
-        executable="move_joint_server",
+        executable="move_arm",
         output='screen'
     )
 
+
     start_world = ExecuteProcess(
         cmd=['ign gazebo', leo_model, '-r'],
         output='screen',
@@ -74,7 +81,7 @@ def generate_launch_description():
 
     load_canadarm_joint_controller = ExecuteProcess(
         cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'canadarm_joint_controller'],
+             'canadarm_joint_trajectory_controller'],
         output='screen'
     )
 
@@ -84,7 +91,8 @@ def generate_launch_description():
         start_world,
         robot_state_publisher,
         spawn,
-        run_node,
+        #run_node,
+        run_move_arm,
 
         RegisterEventHandler(
             OnProcessExit(
@@ -98,4 +106,4 @@ def generate_launch_description():
                 on_exit=[load_canadarm_joint_controller],
             )
         ),
-    ])
+    ])

canadarm/nodes/move_arm

@@ -0,0 +1,76 @@
+#!/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
+from std_srvs.srv import Empty
+
+class MoveArm(Node):
+
+    def __init__(self):
+        super().__init__('arm_node')
+        self.arm_publisher_ = self.create_publisher(JointTrajectory, '/canadarm_joint_trajectory_controller/joint_trajectory', 10)
+        self.open_srv = self.create_service(Empty, 'open_arm', self.open_arm_callback)
+        self.close_srv = self.create_service(Empty, 'close_arm', self.close_arm_callback)
+        self.random_srv = self.create_service(Empty, 'random_arm', self.random_arm_callback)
+
+
+    def open_arm_callback(self, request, response):
+        traj = JointTrajectory()        
+        traj.joint_names = ["Base_Joint", "Shoulder_Roll", "Shoulder_Yaw", "Elbow_Pitch", "Wrist_Pitch", "Wrist_Yaw", "Wrist_Roll"]
+
+        point1 = JointTrajectoryPoint()
+        point1.positions = [0.0, 0.0, 0.0, -3.1416, 0.0, 0.0, 0.0]
+        point1.time_from_start = Duration(sec=4)
+
+        traj.points.append(point1)
+        self.arm_publisher_.publish(traj)
+
+
+        return response
+
+    def close_arm_callback(self, request, response):
+        traj = JointTrajectory()
+        traj.joint_names = ["Base_Joint", "Shoulder_Roll", "Shoulder_Yaw", "Elbow_Pitch", "Wrist_Pitch", "Wrist_Yaw", "Wrist_Roll"]
+
+        point1 = JointTrajectoryPoint()
+        point1.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        point1.time_from_start = Duration(sec=4)
+
+        traj.points.append(point1)
+        self.arm_publisher_.publish(traj)
+
+        return response
+
+    def random_arm_callback(self, request, response):
+        traj = JointTrajectory()
+        traj.joint_names = ["Base_Joint", "Shoulder_Roll", "Shoulder_Yaw", "Elbow_Pitch", "Wrist_Pitch", "Wrist_Yaw", "Wrist_Roll"]
+
+        point1 = JointTrajectoryPoint()
+        point1.positions = [1.0, -1.5, 2.0, -3.2, 0.8, 0.5, -1.0]
+        point1.time_from_start = Duration(sec=4)
+
+        traj.points.append(point1)
+        self.arm_publisher_.publish(traj)
+
+        return response
+
+
+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()

canadarm/worlds/simple.world

@@ -47,7 +47,7 @@
           <visual name='visual'>
             <geometry>
               <mesh>
-                <uri>/home/spaceros-user/demos_ws/install/simulation/share/simulation/models/canadarm/meshes/earth.dae</uri>
+                <uri>model://canadarm/meshes/earth.dae</uri>
                 <scale>3 3 3</scale>
               </mesh>
             </geometry>
@@ -77,7 +77,7 @@
         <visual name='visual'>
           <geometry>
             <mesh>
-              <uri>/home/spaceros-user/demos_ws/install/simulation/share/simulation/models/canadarm/meshes/iss.dae</uri>
+              <uri>model://canadarm/meshes/iss.dae</uri>
               <scale>1 1 1</scale>
             </mesh>
           </geometry>