control arm

element-robotics · Jun 17, 2022 · 51c8225 · 51c8225
1 parent 9febf1a
commit 51c8225
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 4 deletions.
diff --git a/mars_rover/config/mars_rover_control.yaml b/mars_rover/config/mars_rover_control.yaml
@@ -13,6 +13,10 @@ joint_trajectory_controller:
   ros__parameters:
     joints:
       - arm_01_joint
+      - arm_02_joint
+      - arm_03_joint
+      - arm_04_joint
+      - arm_tools_joint
     interface_name: position
     command_interfaces:
       - position

diff --git a/mars_rover/launch/mars_rover.launch.py b/mars_rover/launch/mars_rover.launch.py
@@ -27,10 +27,11 @@ def generate_launch_description():
 
     test_node = Node(
         package="mars_rover",
-        executable="test_node",
+        executable="move_arm",
         parameters=[
             {"robot_description": Command(['xacro ', LaunchConfiguration('model')])}
-        ]
+        ],
+        output='screen'
     )
 
     start_world = ExecuteProcess(

diff --git a/mars_rover/nodes/move_arm b/mars_rover/nodes/move_arm
@@ -2,20 +2,31 @@
 
 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 MoveArm(Node):
 
     def __init__(self):
         super().__init__('arm_node')
-        self.arm1_publisher_ = self.create_publisher(Float64, 'topic', 10)
+        self.arm1_publisher_ = self.create_publisher(JointTrajectory, '/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 = ["arm_01_joint", "arm_02_joint", "arm_03_joint", "arm_04_joint", "arm_tools_joint"]
+
+        point = JointTrajectoryPoint()
+        point.positions = [0.0,0.0,0.0,0.0,0.0]
+        point.time_from_start = Duration(sec=1)
+
+        traj.points.append(point)
+        self.arm1_publisher_.publish(traj)
+
 
 def main(args=None):
     rclpy.init(args=args)

diff --git a/mars_rover/urdf/curiosity_mars_rover.xacro b/mars_rover/urdf/curiosity_mars_rover.xacro
@@ -36,6 +36,30 @@
             <state_interface name="position" />
             <state_interface name="velocity" />
         </joint>
+        <joint name="arm_02_joint">
+            <command_interface name="position">
+            </command_interface>
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="arm_03_joint">
+            <command_interface name="position">
+            </command_interface>
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="arm_04_joint">
+            <command_interface name="position">
+            </command_interface>
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
+        <joint name="arm_tools_joint">
+            <command_interface name="position">
+            </command_interface>
+            <state_interface name="position" />
+            <state_interface name="velocity" />
+        </joint>
     </ros2_control>
 
     <gazebo>