テスト時はダミーのパラメータファイルを読み込めるようにdescription_loaderを修正

rt-net · Sep 7, 2023 · 80016fd · 80016fd
1 parent bf67052
commit 80016fd
Show file tree

Hide file tree

Showing 6 changed files with 57 additions and 4 deletions.
diff --git a/crane_plus_description/crane_plus_description/robot_description_loader.py b/crane_plus_description/crane_plus_description/robot_description_loader.py
@@ -29,6 +29,8 @@ def __init__(self):
         self.use_gazebo = 'false'
         self.use_ignition = 'false'
         self.use_camera = 'false'
+        self.gz_control_config_package = ''
+        self.gz_control_config_file_path = ''
 
     def load(self):
         return Command([
@@ -37,5 +39,7 @@ def load(self):
                 ' port_name:=', self.port_name,
                 ' use_gazebo:=', self.use_gazebo,
                 ' use_ignition:=', self.use_ignition,
-                ' use_camera:=', self.use_camera
+                ' use_camera:=', self.use_camera,
+                ' gz_control_config_package:=', self.gz_control_config_package,
+                ' gz_control_config_file_path:=', self.gz_control_config_file_path
                 ])
diff --git a/crane_plus_description/test/dummy_controllers.yaml b/crane_plus_description/test/dummy_controllers.yaml
@@ -0,0 +1,35 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    crane_plus_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    crane_plus_gripper_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+crane_plus_arm_controller:
+  ros__parameters:
+    joints:
+      - crane_plus_joint1
+      - crane_plus_joint2
+      - crane_plus_joint3
+      - crane_plus_joint4
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+
+crane_plus_gripper_controller:
+  ros__parameters:
+    joints:
+      - crane_plus_joint_hand
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
diff --git a/crane_plus_description/test/test_robot_description_loader.py b/crane_plus_description/test/test_robot_description_loader.py
@@ -50,6 +50,8 @@ def test_use_gazebo():
     # use_gazeboが変更され、xacroにgazebo_ros2_controlがセットされることを期待
     rdl = RobotDescriptionLoader()
     rdl.use_gazebo = 'true'
+    rdl.gz_control_config_package = 'crane_plus_description'
+    rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml'
     assert 'gazebo_ros2_control/GazeboSystem' in exec_load(rdl)
 
 
@@ -58,11 +60,15 @@ def test_use_ignition():
     rdl = RobotDescriptionLoader()
     rdl.use_gazebo = 'true'
     rdl.use_ignition = 'true'
+    rdl.gz_control_config_package = 'crane_plus_description'
+    rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml'
     assert 'ign_ros2_control/IgnitionSystem' in exec_load(rdl)
 
 
 def test_use_camera():
     # use_cameraが変更されて、xacroにcameraがセットされることを期待
     rdl = RobotDescriptionLoader()
     rdl.use_camera = 'true'
+    rdl.gz_control_config_package = 'crane_plus_description'
+    rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml'
     assert 'camera_link' in exec_load(rdl)
diff --git a/crane_plus_description/urdf/crane_plus.gazebo.xacro b/crane_plus_description/urdf/crane_plus.gazebo.xacro
@@ -27,19 +27,21 @@
             name_link_4
             name_link_hand
             use_ignition
+            config_package
+            config_file_path
             ">
 
     <gazebo>
       <xacro:if value="${use_ignition}">
         <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
-          <parameters>$(find crane_plus_control)/config/crane_plus_controllers.yaml</parameters>
+          <parameters>$(find ${config_package})/${config_file_path}</parameters>
         </plugin>
       </xacro:if>
 
       <xacro:unless value="${use_ignition}">
         <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
           <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
-          <parameters>$(find crane_plus_control)/config/crane_plus_controllers.yaml</parameters>
+          <parameters>$(find ${config_package})/${config_file_path}</parameters>
         </plugin>
       </xacro:unless>
     </gazebo>

diff --git a/crane_plus_description/urdf/crane_plus.urdf.xacro b/crane_plus_description/urdf/crane_plus.urdf.xacro
@@ -22,6 +22,8 @@
   <xacro:arg name="read_loads" default="0" />
   <xacro:arg name="read_voltages" default="0" />
   <xacro:arg name="read_temperatures" default="0" />
+  <xacro:arg name="gz_control_config_package" default="" />
+  <xacro:arg name="gz_control_config_file_path" default="" />
 
   <!-- Link to provide the location reference for the arm -->
   <link name="base_link"/>
@@ -128,7 +130,9 @@
       name_link_3="${NAME_LINK_3}"
       name_link_4="${NAME_LINK_4}"
       name_link_hand="${NAME_LINK_HAND}"
-      use_ignition="$(arg use_ignition)" />
+      use_ignition="$(arg use_ignition)"
+      config_package="$(arg gz_control_config_package)" 
+      config_file_path="$(arg gz_control_config_file_path)" />
 
     <xacro:crane_plus_gazebo_ros2_control_settings 
       name="crane_plus"

diff --git a/crane_plus_gazebo/launch/crane_plus_with_table.launch.py b/crane_plus_gazebo/launch/crane_plus_with_table.launch.py
@@ -54,6 +54,8 @@ def generate_launch_description():
     description_loader = RobotDescriptionLoader()
     description_loader.use_gazebo = 'true'
     description_loader.use_ignition = 'true'
+    description_loader.gz_control_config_package = 'crane_plus_control'
+    description_loader.gz_control_config_file_path = 'config/crane_plus_controllers.yaml'
     description = description_loader.load()
 
     move_group = IncludeLaunchDescription(