Support ROS 2

CMakeLists.txt

@@ -21,7 +21,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_pytest REQUIRED)
-  # ament_add_pytest_test(test_robot_description_loader test/test_robot_description_loader.py)
+  ament_add_pytest_test(test_robot_description_loader test/test_robot_description_loader.py)
 endif()
 
 ament_package()

package.xml

@@ -9,6 +9,7 @@
 
   <author email="[email protected]">Hiroyuki Nomura</author>
   <author email="[email protected]">ShotaAk</author>
+  <author email="[email protected]">Atsushi Kuwagata</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

test/test_robot_description_loader.py

@@ -0,0 +1,27 @@
+# Copyright 2023 RT Corporation
+
+from sciurus17_description.robot_description_loader import RobotDescriptionLoader
+from launch.launch_context import LaunchContext
+import pytest
+
+
+def exec_load(loader):
+    # Command substitutionの実行方法はCommandのテストを参考にした
+    # https://github.com/ros2/launch/blob/074cd2903ddccd61bce8f40a0f58da0b7c200481/launch/test/launch/substitutions/test_command.py#L47
+    context = LaunchContext()
+    return loader.load().perform(context)
+
+
+def test_load_description():
+    # xacroの読み込みが成功することを期待
+    rdl = RobotDescriptionLoader()
+    assert exec_load(rdl)
+
+
+def test_change_description_path():
+    # xacroのファイルパスを変更し、読み込みが失敗することを期待
+    rdl = RobotDescriptionLoader()
+    rdl.robot_description_path = 'hoge'
+    with pytest.raises(Exception) as e:
+        exec_load(rdl)
+    assert e.value

urdf/sciurus17.urdf.xacro

@@ -21,7 +21,7 @@
   <xacro:property name="NAME_LINK_NECK_1" value="neck_yaw_link"/>
   <xacro:property name="NAME_LINK_NECK_2" value="neck_pitch_link"/>
 
-  <xacro:property name="NAME_LINK_CAMERA" value="camera_link"/>
+  <xacro:property name="NAME_LINK_HEAD_CAMERA" value="camera_link"/>
   <xacro:property name="NAME_LINK_CHEST_CAMERA" value="chest_camera_link"/>
 
   <xacro:property name="NAME_LINK_ARM_R_1" value="r_link1"/>
@@ -52,7 +52,7 @@
   <xacro:property name="NAME_JOINT_NECK_1" value="neck_yaw_joint"/>
   <xacro:property name="NAME_JOINT_NECK_2" value="neck_pitch_joint"/>
 
-  <xacro:property name="NAME_JOINT_CAMERA" value="head_camera_joint"/>
+  <xacro:property name="NAME_JOINT_HEAD_CAMERA" value="head_camera_joint"/>
   <xacro:property name="NAME_JOINT_CHEST_CAMERA" value="chest_camera_joint"/>
 
   <xacro:property name="NAME_JOINT_ARM_R_1" value="r_arm_joint1"/>
@@ -186,81 +186,16 @@
     <child link="${NAME_LINK_BASE}"/>
   </joint>
 
-  <xacro:sciurus17_body
-    waist_yaw_upper_limit="${NAME_JOINT_BODY_LOWER_LIMIT}"
-    waist_yaw_lower_limit="${NAME_JOINT_BODY_UPPER_LIMIT}"
-    waist_yaw_effort_limit="${EFFORT_LIMIT}"
-    waist_yaw_velocity_limit="${VELOCITY_LIMIT}"
-    base_color="${COLOR_LINK_BASE}"
-    body_color="${COLOR_LINK_BODY}"
-    camera_color="${COLOR_LINK_CAMERA}">
-  </xacro:sciurus17_body>
-
-  <xacro:sciurus17_head
-    neck_yaw_lower_limit="${JOINT_NECK_1_LOWER_LIMIT}"
-    neck_yaw_upper_limit="${JOINT_NECK_1_UPPER_LIMIT}"
-    neck_pitch_lower_limit="${JOINT_NECK_2_LOWER_LIMIT}"
-    neck_pitch_upper_limit="${JOINT_NECK_2_UPPER_LIMIT}"
-    neck_joints_effort_limit="${EFFORT_LIMIT}"
-    neck_joints_velocity_limit="${VELOCITY_LIMIT}"
-    head_color="${COLOR_LINK_HEAD}"
-    camera_color="${COLOR_LINK_CAMERA}">
-  </xacro:sciurus17_head>
-
-  <xacro:sciurus17_right_arm
-    arm_joint1_lower_limit="${JOINT_ARM_R_1_LOWER_LIMIT}"
-    arm_joint1_upper_limit="${JOINT_ARM_R_1_UPPER_LIMIT}"
-    arm_joint2_lower_limit="${JOINT_ARM_R_2_LOWER_LIMIT}"
-    arm_joint2_upper_limit="${JOINT_ARM_R_2_UPPER_LIMIT}"
-    arm_joint3_lower_limit="${JOINT_ARM_R_3_LOWER_LIMIT}"
-    arm_joint3_upper_limit="${JOINT_ARM_R_3_UPPER_LIMIT}"
-    arm_joint4_lower_limit="${JOINT_ARM_R_4_LOWER_LIMIT}"
-    arm_joint4_upper_limit="${JOINT_ARM_R_4_UPPER_LIMIT}"
-    arm_joint5_lower_limit="${JOINT_ARM_R_5_LOWER_LIMIT}"
-    arm_joint5_upper_limit="${JOINT_ARM_R_5_UPPER_LIMIT}"
-    arm_joint6_lower_limit="${JOINT_ARM_R_6_LOWER_LIMIT}"
-    arm_joint6_upper_limit="${JOINT_ARM_R_6_UPPER_LIMIT}"
-    arm_joint7_lower_limit="${JOINT_ARM_R_7_LOWER_LIMIT}"
-    arm_joint7_upper_limit="${JOINT_ARM_R_7_UPPER_LIMIT}"
-    arm_joints_effort_limit="${EFFORT_LIMIT}"
-    arm_joints_velocity_limit="${VELOCITY_LIMIT}"
-    arm_color="${COLOR_LINK_ARM}">
-  </xacro:sciurus17_right_arm>
-
-  <xacro:sciurus17_right_hand
-    hand_joints_lower_limit="${JOINT_HAND_R_LOWER_LIMIT}"
-    hand_joints_upper_limit="${JOINT_HAND_R_UPPER_LIMIT}"
-    hand_joints_effort_limit="${EFFORT_LIMIT}"
-    hand_joints_velocity_limit="${VELOCITY_LIMIT}"
-    hand_color="${COLOR_LINK_HAND}">
-  </xacro:sciurus17_right_hand>
-
-  <xacro:sciurus17_left_arm
-    arm_joint1_lower_limit="${JOINT_ARM_L_1_LOWER_LIMIT}"
-    arm_joint1_upper_limit="${JOINT_ARM_L_1_UPPER_LIMIT}"
-    arm_joint2_lower_limit="${JOINT_ARM_L_2_LOWER_LIMIT}"
-    arm_joint2_upper_limit="${JOINT_ARM_L_2_UPPER_LIMIT}"
-    arm_joint3_lower_limit="${JOINT_ARM_L_3_LOWER_LIMIT}"
-    arm_joint3_upper_limit="${JOINT_ARM_L_3_UPPER_LIMIT}"
-    arm_joint4_lower_limit="${JOINT_ARM_L_4_LOWER_LIMIT}"
-    arm_joint4_upper_limit="${JOINT_ARM_L_4_UPPER_LIMIT}"
-    arm_joint5_lower_limit="${JOINT_ARM_L_5_LOWER_LIMIT}"
-    arm_joint5_upper_limit="${JOINT_ARM_L_5_UPPER_LIMIT}"
-    arm_joint6_lower_limit="${JOINT_ARM_L_6_LOWER_LIMIT}"
-    arm_joint6_upper_limit="${JOINT_ARM_L_6_UPPER_LIMIT}"
-    arm_joint7_lower_limit="${JOINT_ARM_L_7_LOWER_LIMIT}"
-    arm_joint7_upper_limit="${JOINT_ARM_L_7_UPPER_LIMIT}"
-    arm_joints_effort_limit="${EFFORT_LIMIT}"
-    arm_joints_velocity_limit="${VELOCITY_LIMIT}"
-    arm_color="${COLOR_LINK_ARM}">
-  </xacro:sciurus17_left_arm>
-
-  <xacro:sciurus17_left_hand
-    hand_joints_lower_limit="${JOINT_HAND_L_LOWER_LIMIT}"
-    hand_joints_upper_limit="${JOINT_HAND_L_UPPER_LIMIT}"
-    hand_joints_effort_limit="${EFFORT_LIMIT}"
-    hand_joints_velocity_limit="${VELOCITY_LIMIT}"
-    hand_color="${COLOR_LINK_HAND}">
-  </xacro:sciurus17_left_hand>
+  <xacro:sciurus17_body/>
+
+  <xacro:sciurus17_head/>
+
+  <xacro:sciurus17_right_arm/>
+
+  <xacro:sciurus17_right_hand/>
+
+  <xacro:sciurus17_left_arm/>
+
+  <xacro:sciurus17_left_hand/>
 
 </robot>

urdf/sciurus17_body.xacro

@@ -2,22 +2,14 @@
 
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
 
-  <xacro:macro name="sciurus17_body"
-    params="waist_yaw_upper_limit
-            waist_yaw_lower_limit
-            waist_yaw_effort_limit
-            waist_yaw_velocity_limit
-            base_color
-            body_color
-            camera_color
-    ">
+  <xacro:macro name="sciurus17_body">
 
     <link name="${NAME_LINK_BASE}">
       <visual>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/Base.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${base_color}"/>
+        <material name="${COLOR_LINK_BASE}"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -37,10 +29,10 @@
       <child link="${NAME_LINK_BODY}" />
       <origin xyz="0.0 0.0 0.1315" rpy="0 0 0"/>
       <limit 
-        lower="${waist_yaw_upper_limit}" 
-        upper="${waist_yaw_lower_limit}" 
-        effort="${waist_yaw_effort_limit}" 
-        velocity="${waist_yaw_velocity_limit}"/>
+        lower="${NAME_JOINT_BODY_LOWER_LIMIT}" 
+        upper="${NAME_JOINT_BODY_UPPER_LIMIT}" 
+        effort="${EFFORT_LIMIT}" 
+        velocity="${VELOCITY_LIMIT}"/>
       <axis xyz="0 0 1"/>
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
@@ -50,7 +42,7 @@
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/Body.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${body_color}"/>
+        <material name="${COLOR_LINK_BODY}"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -77,7 +69,7 @@
         <geometry>
           <cylinder length="0.002" radius="0.003"/>
         </geometry>
-        <material name="${camera_color}"/>
+        <material name="${COLOR_LINK_CAMERA}"/>
       </visual>
       <collision>
         <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>

urdf/sciurus17_head.xacro

@@ -2,26 +2,17 @@
 
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
 
-  <xacro:macro name="sciurus17_head" 
-    params="neck_yaw_lower_limit
-            neck_yaw_upper_limit
-            neck_pitch_lower_limit
-            neck_pitch_upper_limit
-            neck_joints_effort_limit
-            neck_joints_velocity_limit
-            head_color
-            camera_color
-            ">
+  <xacro:macro name="sciurus17_head">
 
     <joint name="${NAME_JOINT_NECK_1}" type="revolute">
       <parent link="${NAME_LINK_BODY}" />
       <child link="${NAME_LINK_NECK_1}" />
       <origin xyz="0.08083 0.0 0.3395" rpy="0 0 0"/>
       <limit 
-        lower="${neck_yaw_lower_limit}" 
-        upper="${neck_yaw_upper_limit}" 
-        effort="${neck_joints_effort_limit}" 
-        velocity="${neck_joints_velocity_limit}"/>
+        lower="${JOINT_NECK_1_LOWER_LIMIT}" 
+        upper="${JOINT_NECK_1_UPPER_LIMIT}" 
+        effort="${EFFORT_LIMIT}" 
+        velocity="${VELOCITY_LIMIT}"/>
       <axis xyz="0 0 1"/>
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
@@ -31,7 +22,7 @@
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/Neck1.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${head_color}"/>
+        <material name="${COLOR_LINK_HEAD}"/>
       </visual>
       <collision>
         <geometry>
@@ -50,10 +41,10 @@
       <child link="${NAME_LINK_NECK_2}" />
       <origin xyz="0.0 0.0 0.054" rpy="0 0 0"/>
       <limit 
-        lower="${neck_pitch_lower_limit}" 
-        upper="${neck_pitch_upper_limit}" 
-        effort="${neck_joints_effort_limit}" 
-        velocity="${neck_joints_velocity_limit}"/>
+        lower="${JOINT_NECK_2_LOWER_LIMIT}" 
+        upper="${JOINT_NECK_2_UPPER_LIMIT}" 
+        effort="${EFFORT_LIMIT}" 
+        velocity="${VELOCITY_LIMIT}"/>
       <axis xyz="0 -1 0"/>
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
@@ -64,7 +55,7 @@
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/Neck2.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${head_color}"/>
+        <material name="${COLOR_LINK_HEAD}"/>
       </visual>
       <collision>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
@@ -79,19 +70,19 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_CAMERA}" type="fixed">
+    <joint name="${NAME_JOINT_HEAD_CAMERA}" type="fixed">
       <parent link="${NAME_LINK_NECK_2}" />
-      <child link="${NAME_LINK_CAMERA}" />
+      <child link="${NAME_LINK_HEAD_CAMERA}" />
       <origin xyz="0.081785 0.020 0.091" rpy="0 0 0"/>
     </joint>
 
-    <link name="${NAME_LINK_CAMERA}">
+    <link name="${NAME_LINK_HEAD_CAMERA}">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
           <cylinder length="0.002" radius="0.003"/>
         </geometry>
-        <material name="${camera_color}"/>
+        <material name="${COLOR_LINK_CAMERA}"/>
       </visual>
       <collision>
         <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
@@ -107,7 +98,7 @@
     <gazebo reference="${NAME_LINK_NECK_2}">
       <material>Gazebo/White</material>
     </gazebo>
-    <gazebo reference="${NAME_LINK_CAMERA}">
+    <gazebo reference="${NAME_LINK_HEAD_CAMERA}">
       <material>Gazebo/Black</material>
     </gazebo>