Merge pull request #16 from small-thinking/add-more-joints

Add more joints

mnlm/robot/robot_arm_ws/src/robot_arm/config/robot_arm_control.yaml

@@ -13,6 +13,11 @@ joint_trajectory_controller:
     joints:
       - servo0
       - servo1
+      - servo2
+      - servo3
+      - servo4
+      - left_gripper_joint
+      - right_gripper_joint
     interface_name: position
     command_interfaces:
       - position

mnlm/robot/robot_arm_ws/src/robot_arm/launch/robot_arm_rviz.launch.py

@@ -4,40 +4,38 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
+
 def generate_launch_description():
     # Define the path to the XACRO file
-    pkg_share = get_package_share_directory('robot_arm')
+    pkg_share = get_package_share_directory("robot_arm")
     # xacro_file = os.path.join(pkg_share, 'models', 'example_robot.urdf.xacro')
-    xacro_file = os.path.join(pkg_share, 'models', 'arm.xacro')
-    
+    xacro_file = os.path.join(pkg_share, "models", "arm.xacro")
+
     # Convert XACRO to URDF
     doc = xacro.process_file(xacro_file)
     robot_description = doc.toxml()
-    
+
     # urdf_file_path = os.path.join(pkg_share, 'models', 'arm.urdf')
     # # Load the URDF file content
     # with open(urdf_file_path, 'r') as urdf_file:
     #     robot_description = urdf_file.read()
 
     # Node to publish the robot_description to the parameter server
     robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        output='both',
-        parameters=[{'robot_description': robot_description}]
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
+        parameters=[{"robot_description": robot_description}],
     )
 
     rviz2 = Node(
-        package='rviz2',
-        executable='rviz2',
-        name='rviz2',
-        output='screen',
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="screen",
         # arguments=['-d', os.path.join(pkg_share, 'rviz', 'robot_arm.rviz')],
-        parameters=[{'use_sim_time': True}]
+        parameters=[{"use_sim_time": True}],
     )
 
-    return LaunchDescription([
-        robot_state_publisher,
-        rviz2
-    ])
+    return LaunchDescription([robot_state_publisher, rviz2])

mnlm/robot/robot_arm_ws/src/robot_arm/launch/robot_arm_sim.launch.py

@@ -1,9 +1,11 @@
 import os
+import tempfile
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import ExecuteProcess
 from launch_ros.actions import Node
+from launch.substitutions import Command
 
 
 def generate_launch_description():
@@ -22,12 +24,25 @@ def generate_launch_description():
 
     # Include the project folder
     pkg_robot_arm_gazebo = get_package_share_directory("robot_arm")
-    urdf_file_path = os.path.join(pkg_robot_arm_gazebo, "models", "arm.urdf")
     world_file_path = os.path.join(pkg_robot_arm_gazebo, "models", "world.sdf")
+    xacro_file_path = os.path.join(pkg_robot_arm_gazebo, "models", "arm.xacro")
 
-    # Read the URDF file
-    with open(urdf_file_path, "r") as file:
-        robot_description_content = file.read()
+    # Convert xacro to URDF
+    robot_description_content = Command(["xacro ", xacro_file_path])
+
+    # Create a temporary file to store the converted URDF
+    temp_urdf_file = tempfile.NamedTemporaryFile(delete=False, mode="w", suffix=".urdf")
+    if os.path.exists(temp_urdf_file.name):
+        os.remove(temp_urdf_file.name)
+    urdf_file_path = temp_urdf_file.name
+
+    # Convert XACRO to URDF and write to the temporary file
+    command = f"xacro {xacro_file_path} > {urdf_file_path}"
+    convert_xacro_to_urdf = ExecuteProcess(
+        cmd=[command],
+        shell=True,
+        output="screen",
+    )
 
     # Start Ignition Gazebo with an empty world
     start_gazebo_cmd = ExecuteProcess(
@@ -88,6 +103,7 @@ def generate_launch_description():
     # Start them all
     return LaunchDescription(
         [
+            convert_xacro_to_urdf,
             start_gazebo_cmd,
             spawn_model_cmd,
             robot_state_publisher_node,