15#ifndef ROS2_CONTROL_TEST_ASSETS__DESCRIPTIONS_HPP_
16#define ROS2_CONTROL_TEST_ASSETS__DESCRIPTIONS_HPP_
21namespace ros2_control_test_assets
25<?xml version="1.0" encoding="utf-8"?>
26<robot name="MinimalRobot">
27 <!-- Used for fixing robot -->
29 <joint name="base_joint" type="fixed">
30 <origin rpy="0 0 0" xyz="0 0 0"/>
31 <parent link="world"/>
32 <child link="base_link"/>
34 <link name="base_link">
38 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
41 <origin rpy="0 0 0" xyz="0 0 0"/>
43 <cylinder length="0.2" radius="0.1"/>
45 <material name="DarkGrey">
46 <color rgba="0.4 0.4 0.4 1.0"/>
50 <origin rpy="0 0 0" xyz="0 0 0"/>
52 <cylinder length="1" radius="0.1"/>
56 <joint name="joint1" type="revolute">
57 <origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
58 <parent link="base_link"/>
60 <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
66 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
69 <origin rpy="0 0 0" xyz="0 0 0"/>
71 <cylinder length="1" radius="0.1"/>
73 <material name="DarkGrey">
74 <color rgba="0.4 0.4 0.4 1.0"/>
78 <origin rpy="0 0 0" xyz="0 0 0"/>
80 <cylinder length="1" radius="0.1"/>
84 <joint name="joint2" type="revolute">
85 <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
86 <parent link="link1"/>
88 <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
89 <safety_controller soft_lower_limit="-1.5" soft_upper_limit="0.5" k_position="10.0" k_velocity="20.0"/>
95 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
98 <origin rpy="0 0 0" xyz="0 0 0"/>
100 <cylinder length="1" radius="0.1"/>
102 <material name="DarkGrey">
103 <color rgba="0.4 0.4 0.4 1.0"/>
107 <origin rpy="0 0 0" xyz="0 0 0"/>
109 <cylinder length="1" radius="0.1"/>
113 <joint name="joint3" type="revolute">
114 <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
115 <parent link="link2"/>
116 <child link="link3"/>
117 <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
122 <origin xyz="0 0 0"/>
123 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
126 <origin rpy="0 0 0" xyz="0 0 0"/>
128 <cylinder length="1" radius="0.1"/>
130 <material name="DarkGrey">
131 <color rgba="0.4 0.4 0.4 1.0"/>
135 <origin rpy="0 0 0" xyz="0 0 0"/>
137 <cylinder length="1" radius="0.1"/>
141 <joint name="tool_joint" type="fixed">
142 <origin rpy="0 0 0" xyz="0 0 1"/>
143 <parent link="link2"/>
144 <child link="tool_link"/>
146 <link name="tool_link">
150const auto urdf_head_revolute_missing_limits =
152<?xml version="1.0" encoding="utf-8"?>
153<robot name="MinimalRobot">
154 <!-- Used for fixing robot -->
156 <joint name="base_joint" type="fixed">
157 <origin rpy="0 0 0" xyz="0 0 0"/>
158 <parent link="world"/>
159 <child link="base_link"/>
161 <link name="base_link">
164 <origin xyz="0 0 0"/>
165 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
168 <origin rpy="0 0 0" xyz="0 0 0"/>
170 <cylinder length="0.2" radius="0.1"/>
172 <material name="DarkGrey">
173 <color rgba="0.4 0.4 0.4 1.0"/>
177 <origin rpy="0 0 0" xyz="0 0 0"/>
179 <cylinder length="1" radius="0.1"/>
183 <joint name="joint1" type="revolute">
184 <origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
185 <parent link="base_link"/>
186 <child link="link1"/>
191 <origin xyz="0 0 0"/>
192 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
195 <origin rpy="0 0 0" xyz="0 0 0"/>
197 <cylinder length="1" radius="0.1"/>
199 <material name="DarkGrey">
200 <color rgba="0.4 0.4 0.4 1.0"/>
204 <origin rpy="0 0 0" xyz="0 0 0"/>
206 <cylinder length="1" radius="0.1"/>
210 <joint name="joint2" type="revolute">
211 <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
212 <parent link="link1"/>
213 <child link="link2"/>
214 <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
219 <origin xyz="0 0 0"/>
220 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
223 <origin rpy="0 0 0" xyz="0 0 0"/>
225 <cylinder length="1" radius="0.1"/>
227 <material name="DarkGrey">
228 <color rgba="0.4 0.4 0.4 1.0"/>
232 <origin rpy="0 0 0" xyz="0 0 0"/>
234 <cylinder length="1" radius="0.1"/>
240const auto urdf_head_continuous_missing_limits =
242<?xml version="1.0" encoding="utf-8"?>
243<robot name="MinimalRobot">
244 <!-- Used for fixing robot -->
246 <joint name="base_joint" type="fixed">
247 <origin rpy="0 0 0" xyz="0 0 0"/>
248 <parent link="world"/>
249 <child link="base_link"/>
251 <link name="base_link">
254 <origin xyz="0 0 0"/>
255 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
258 <origin rpy="0 0 0" xyz="0 0 0"/>
260 <cylinder length="0.2" radius="0.1"/>
262 <material name="DarkGrey">
263 <color rgba="0.4 0.4 0.4 1.0"/>
267 <origin rpy="0 0 0" xyz="0 0 0"/>
269 <cylinder length="1" radius="0.1"/>
273 <joint name="joint1" type="continuous">
274 <origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
275 <parent link="base_link"/>
276 <child link="link1"/>
281 <origin xyz="0 0 0"/>
282 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
285 <origin rpy="0 0 0" xyz="0 0 0"/>
287 <cylinder length="1" radius="0.1"/>
289 <material name="DarkGrey">
290 <color rgba="0.4 0.4 0.4 1.0"/>
294 <origin rpy="0 0 0" xyz="0 0 0"/>
296 <cylinder length="1" radius="0.1"/>
300 <joint name="joint2" type="continuous">
301 <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
302 <parent link="link1"/>
303 <child link="link2"/>
308 <origin xyz="0 0 0"/>
309 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
312 <origin rpy="0 0 0" xyz="0 0 0"/>
314 <cylinder length="1" radius="0.1"/>
316 <material name="DarkGrey">
317 <color rgba="0.4 0.4 0.4 1.0"/>
321 <origin rpy="0 0 0" xyz="0 0 0"/>
323 <cylinder length="1" radius="0.1"/>
327 <joint name="joint3" type="revolute">
328 <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
329 <parent link="link2"/>
330 <child link="link3"/>
331 <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
336 <origin xyz="0 0 0"/>
337 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
340 <origin rpy="0 0 0" xyz="0 0 0"/>
342 <cylinder length="1" radius="0.1"/>
344 <material name="DarkGrey">
345 <color rgba="0.4 0.4 0.4 1.0"/>
349 <origin rpy="0 0 0" xyz="0 0 0"/>
351 <cylinder length="1" radius="0.1"/>
355 <joint name="tool_joint" type="fixed">
356 <origin rpy="0 0 0" xyz="0 0 1"/>
357 <parent link="link2"/>
358 <child link="tool_link"/>
360 <link name="tool_link">
364const auto urdf_head_continuous_with_limits =
366<?xml version="1.0" encoding="utf-8"?>
367<robot name="MinimalRobot">
368 <!-- Used for fixing robot -->
370 <joint name="base_joint" type="fixed">
371 <origin rpy="0 0 0" xyz="0 0 0"/>
372 <parent link="world"/>
373 <child link="base_link"/>
375 <link name="base_link">
378 <origin xyz="0 0 0"/>
379 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
382 <origin rpy="0 0 0" xyz="0 0 0"/>
384 <cylinder length="0.2" radius="0.1"/>
386 <material name="DarkGrey">
387 <color rgba="0.4 0.4 0.4 1.0"/>
391 <origin rpy="0 0 0" xyz="0 0 0"/>
393 <cylinder length="1" radius="0.1"/>
397 <joint name="joint1" type="continuous">
398 <origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
399 <parent link="base_link"/>
400 <child link="link1"/>
401 <limit effort="0.1" velocity="0.2"/>
406 <origin xyz="0 0 0"/>
407 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
410 <origin rpy="0 0 0" xyz="0 0 0"/>
412 <cylinder length="1" radius="0.1"/>
414 <material name="DarkGrey">
415 <color rgba="0.4 0.4 0.4 1.0"/>
419 <origin rpy="0 0 0" xyz="0 0 0"/>
421 <cylinder length="1" radius="0.1"/>
425 <joint name="joint2" type="continuous">
426 <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
427 <parent link="link1"/>
428 <child link="link2"/>
429 <limit effort="0.1" velocity="0.2"/>
434 <origin xyz="0 0 0"/>
435 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
438 <origin rpy="0 0 0" xyz="0 0 0"/>
440 <cylinder length="1" radius="0.1"/>
442 <material name="DarkGrey">
443 <color rgba="0.4 0.4 0.4 1.0"/>
447 <origin rpy="0 0 0" xyz="0 0 0"/>
449 <cylinder length="1" radius="0.1"/>
455const auto urdf_head_prismatic_missing_limits =
457<?xml version="1.0" encoding="utf-8"?>
458<robot name="MinimalRobot">
459 <!-- Used for fixing robot -->
461 <joint name="base_joint" type="fixed">
462 <origin rpy="0 0 0" xyz="0 0 0"/>
463 <parent link="world"/>
464 <child link="base_link"/>
466 <link name="base_link">
469 <origin xyz="0 0 0"/>
470 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
473 <origin rpy="0 0 0" xyz="0 0 0"/>
475 <cylinder length="0.2" radius="0.1"/>
477 <material name="DarkGrey">
478 <color rgba="0.4 0.4 0.4 1.0"/>
482 <origin rpy="0 0 0" xyz="0 0 0"/>
484 <cylinder length="1" radius="0.1"/>
488 <joint name="joint1" type="prismatic">
489 <origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
490 <parent link="base_link"/>
491 <child link="link1"/>
496 <origin xyz="0 0 0"/>
497 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
500 <origin rpy="0 0 0" xyz="0 0 0"/>
502 <cylinder length="1" radius="0.1"/>
504 <material name="DarkGrey">
505 <color rgba="0.4 0.4 0.4 1.0"/>
509 <origin rpy="0 0 0" xyz="0 0 0"/>
511 <cylinder length="1" radius="0.1"/>
515 <joint name="joint2" type="revolute">
516 <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
517 <parent link="link1"/>
518 <child link="link2"/>
519 <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
524 <origin xyz="0 0 0"/>
525 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
528 <origin rpy="0 0 0" xyz="0 0 0"/>
530 <cylinder length="1" radius="0.1"/>
532 <material name="DarkGrey">
533 <color rgba="0.4 0.4 0.4 1.0"/>
537 <origin rpy="0 0 0" xyz="0 0 0"/>
539 <cylinder length="1" radius="0.1"/>
545const auto urdf_head_mimic =
547<?xml version="1.0" encoding="utf-8"?>
548<robot name="MinimalRobot">
549 <!-- Used for fixing robot -->
551 <joint name="base_joint" type="fixed">
552 <origin rpy="0 0 0" xyz="0 0 0"/>
553 <parent link="world"/>
554 <child link="base_link"/>
556 <link name="base_link">
559 <origin xyz="0 0 0"/>
560 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
563 <origin rpy="0 0 0" xyz="0 0 0"/>
565 <cylinder length="0.2" radius="0.1"/>
567 <material name="DarkGrey">
568 <color rgba="0.4 0.4 0.4 1.0"/>
572 <origin rpy="0 0 0" xyz="0 0 0"/>
574 <cylinder length="1" radius="0.1"/>
578 <joint name="joint1" type="revolute">
579 <origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
580 <parent link="base_link"/>
581 <child link="link1"/>
582 <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
587 <origin xyz="0 0 0"/>
588 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
591 <origin rpy="0 0 0" xyz="0 0 0"/>
593 <cylinder length="1" radius="0.1"/>
595 <material name="DarkGrey">
596 <color rgba="0.4 0.4 0.4 1.0"/>
600 <origin rpy="0 0 0" xyz="0 0 0"/>
602 <cylinder length="1" radius="0.1"/>
606 <joint name="joint2" type="revolute">
607 <mimic joint="joint1" multiplier="-2" offset="0"/>
608 <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
609 <parent link="link1"/>
610 <child link="link2"/>
611 <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
616 <origin xyz="0 0 0"/>
617 <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
620 <origin rpy="0 0 0" xyz="0 0 0"/>
622 <cylinder length="1" radius="0.1"/>
624 <material name="DarkGrey">
625 <color rgba="0.4 0.4 0.4 1.0"/>
629 <origin rpy="0 0 0" xyz="0 0 0"/>
631 <cylinder length="1" radius="0.1"/>
635 <joint name="tool_joint" type="fixed">
636 <origin rpy="0 0 0" xyz="0 0 1"/>
637 <parent link="link2"/>
638 <child link="tool_link"/>
640 <link name="tool_link">
644const auto urdf_tail =
649const auto hardware_resources =
651 <ros2_control name="TestActuatorHardware" type="actuator">
653 <plugin>test_actuator</plugin>
655 <joint name="joint1">
656 <command_interface name="position"/>
657 <state_interface name="position"/>
658 <state_interface name="velocity"/>
659 <command_interface name="max_velocity" />
662 <ros2_control name="TestSensorHardware" type="sensor">
664 <plugin>test_sensor</plugin>
665 <param name="example_param_write_for_sec">2</param>
666 <param name="example_param_read_for_sec">2</param>
668 <sensor name="sensor1">
669 <state_interface name="velocity"/>
672 <ros2_control name="TestSystemHardware" type="system">
674 <plugin>test_system</plugin>
675 <param name="example_param_write_for_sec">2</param>
676 <param name="example_param_read_for_sec">2</param>
678 <joint name="joint2">
679 <command_interface name="velocity"/>
680 <state_interface name="position"/>
681 <state_interface name="velocity"/>
682 <state_interface name="acceleration"/>
683 <command_interface name="max_acceleration" />
685 <joint name="joint3">
686 <command_interface name="velocity"/>
687 <state_interface name="position"/>
688 <state_interface name="velocity"/>
689 <state_interface name="acceleration"/>
691 <gpio name="configuration">
692 <command_interface name="max_tcp_jerk"/>
693 <state_interface name="max_tcp_jerk"/>
698const auto async_hardware_resources =
700 <ros2_control name="TestActuatorHardware" type="actuator" is_async="true" thread_priority="30">
702 <async affinity="[2, 4,6]" scheduling_policy="detached" print_warnings="false"/>
705 <plugin>test_actuator</plugin>
707 <joint name="joint1">
708 <command_interface name="position"/>
709 <state_interface name="position"/>
710 <state_interface name="velocity"/>
711 <command_interface name="max_velocity" />
714 <ros2_control name="TestSensorHardware" type="sensor" is_async="true">
716 <plugin>test_sensor</plugin>
717 <param name="example_param_write_for_sec">2</param>
718 <param name="example_param_read_for_sec">2</param>
720 <sensor name="sensor1">
721 <state_interface name="velocity"/>
724 <ros2_control name="TestSystemHardware" type="system" is_async="true">
726 <async thread_priority="70" affinity="[1]" scheduling_policy="synchronized"/>
729 <plugin>test_system</plugin>
730 <param name="example_param_write_for_sec">2</param>
731 <param name="example_param_read_for_sec">2</param>
733 <joint name="joint2">
734 <command_interface name="velocity"/>
735 <state_interface name="position"/>
736 <state_interface name="velocity"/>
737 <state_interface name="acceleration"/>
738 <command_interface name="max_acceleration" />
740 <joint name="joint3">
741 <command_interface name="velocity"/>
742 <state_interface name="position"/>
743 <state_interface name="velocity"/>
744 <state_interface name="acceleration"/>
746 <gpio name="configuration">
747 <command_interface name="max_tcp_jerk"/>
748 <state_interface name="max_tcp_jerk"/>
753const auto hardware_resources_with_different_rw_rates =
755 <ros2_control name="TestActuatorHardware" type="actuator" rw_rate="50">
757 <plugin>test_actuator</plugin>
759 <joint name="joint1">
760 <command_interface name="position"/>
761 <state_interface name="position"/>
762 <state_interface name="velocity"/>
763 <command_interface name="max_velocity" />
766 <ros2_control name="TestSensorHardware" type="sensor" rw_rate="20">
768 <plugin>test_sensor</plugin>
769 <param name="example_param_write_for_sec">2</param>
770 <param name="example_param_read_for_sec">2</param>
772 <sensor name="sensor1">
773 <state_interface name="velocity"/>
776 <ros2_control name="TestSystemHardware" type="system" rw_rate="25">
778 <plugin>test_system</plugin>
779 <param name="example_param_write_for_sec">2</param>
780 <param name="example_param_read_for_sec">2</param>
782 <joint name="joint2">
783 <command_interface name="velocity"/>
784 <state_interface name="position"/>
785 <state_interface name="velocity"/>
786 <state_interface name="acceleration"/>
787 <command_interface name="max_acceleration" />
789 <joint name="joint3">
790 <command_interface name="velocity"/>
791 <state_interface name="position"/>
792 <state_interface name="velocity"/>
793 <state_interface name="acceleration"/>
795 <gpio name="configuration">
796 <command_interface name="max_tcp_jerk"/>
797 <state_interface name="max_tcp_jerk"/>
802const auto hardware_resources_with_different_rw_rates_with_async =
804 <ros2_control name="TestActuatorHardware" type="actuator" rw_rate="50" is_async="true">
806 <plugin>test_actuator</plugin>
808 <joint name="joint1">
809 <command_interface name="position"/>
810 <state_interface name="position"/>
811 <state_interface name="velocity"/>
812 <command_interface name="max_velocity" />
815 <ros2_control name="TestSensorHardware" type="sensor" rw_rate="20" is_async="true">
817 <plugin>test_sensor</plugin>
818 <param name="example_param_write_for_sec">2</param>
819 <param name="example_param_read_for_sec">2</param>
821 <sensor name="sensor1">
822 <state_interface name="velocity"/>
825 <ros2_control name="TestSystemHardware" type="system" rw_rate="25" is_async="true">
827 <plugin>test_system</plugin>
828 <param name="example_param_write_for_sec">2</param>
829 <param name="example_param_read_for_sec">2</param>
831 <joint name="joint2">
832 <command_interface name="velocity"/>
833 <state_interface name="position"/>
834 <state_interface name="velocity"/>
835 <state_interface name="acceleration"/>
836 <command_interface name="max_acceleration" />
838 <joint name="joint3">
839 <command_interface name="velocity"/>
840 <state_interface name="position"/>
841 <state_interface name="velocity"/>
842 <state_interface name="acceleration"/>
844 <gpio name="configuration">
845 <command_interface name="max_tcp_jerk"/>
846 <state_interface name="max_tcp_jerk"/>
851const auto hardware_resources_with_negative_rw_rates =
853 <ros2_control name="TestActuatorHardware" type="actuator" rw_rate="-50">
855 <plugin>test_actuator</plugin>
857 <joint name="right_finger_joint">
858 <command_interface name="position"/>
859 <state_interface name="position"/>
860 <state_interface name="velocity"/>
861 <command_interface name="max_velocity" />
866const auto hardware_resources_invalid_with_text_in_rw_rates =
868 <ros2_control name="TestActuatorHardware" type="actuator" rw_rate="d 50">
870 <plugin>test_actuator</plugin>
872 <joint name="right_finger_joint">
873 <command_interface name="position"/>
874 <state_interface name="position"/>
875 <state_interface name="velocity"/>
876 <command_interface name="max_velocity" />
881const auto hardware_resources_invalid_out_of_range_in_rw_rates =
883 <ros2_control name="TestActuatorHardware" type="actuator" rw_rate="12345678901">
885 <plugin>test_actuator</plugin>
887 <joint name="right_finger_joint">
888 <command_interface name="position"/>
889 <state_interface name="position"/>
890 <state_interface name="velocity"/>
891 <command_interface name="max_velocity" />
896const auto uninitializable_hardware_resources =
898 <ros2_control name="TestUninitializableActuatorHardware" type="actuator">
900 <plugin>test_uninitializable_actuator</plugin>
902 <joint name="joint1">
903 <command_interface name="position"/>
904 <state_interface name="position"/>
905 <state_interface name="velocity"/>
906 <command_interface name="max_velocity" />
909 <ros2_control name="TestUninitializableSensorHardware" type="sensor">
911 <plugin>test_uninitializable_sensor</plugin>
912 <param name="example_param_write_for_sec">2</param>
913 <param name="example_param_read_for_sec">2</param>
915 <sensor name="sensor1">
916 <state_interface name="velocity"/>
919 <ros2_control name="TestUninitializableSystemHardware" type="system">
921 <plugin>test_uninitializable_system</plugin>
922 <param name="example_param_write_for_sec">2</param>
923 <param name="example_param_read_for_sec">2</param>
925 <joint name="joint2">
926 <command_interface name="velocity"/>
927 <state_interface name="position"/>
928 <state_interface name="velocity"/>
929 <state_interface name="acceleration"/>
930 <command_interface name="max_acceleration" />
932 <joint name="joint3">
933 <command_interface name="velocity"/>
934 <state_interface name="position"/>
935 <state_interface name="velocity"/>
936 <state_interface name="acceleration"/>
941const auto hardware_resources_not_existing_actuator_plugin =
943 <ros2_control name="TestActuatorHardware" type="actuator">
945 <plugin>test_actuator23</plugin>
947 <joint name="joint1">
948 <command_interface name="position"/>
949 <state_interface name="position"/>
950 <state_interface name="velocity"/>
951 <command_interface name="max_velocity" />
954 <ros2_control name="TestSensorHardware" type="sensor">
956 <plugin>test_sensor</plugin>
957 <param name="example_param_write_for_sec">2</param>
958 <param name="example_param_read_for_sec">2</param>
960 <sensor name="sensor1">
961 <state_interface name="velocity"/>
964 <ros2_control name="TestSystemHardware" type="system">
966 <plugin>test_system</plugin>
967 <param name="example_param_write_for_sec">2</param>
968 <param name="example_param_read_for_sec">2</param>
970 <joint name="joint2">
971 <command_interface name="velocity"/>
972 <state_interface name="position"/>
973 <state_interface name="velocity"/>
974 <state_interface name="acceleration"/>
975 <command_interface name="max_acceleration" />
977 <joint name="joint3">
978 <command_interface name="velocity"/>
979 <state_interface name="position"/>
980 <state_interface name="velocity"/>
981 <state_interface name="acceleration"/>
983 <gpio name="configuration">
984 <command_interface name="max_tcp_jerk"/>
985 <state_interface name="max_tcp_jerk"/>
990const auto hardware_resources_not_existing_sensor_plugin =
992 <ros2_control name="TestActuatorHardware" type="actuator">
994 <plugin>test_actuator</plugin>
996 <joint name="joint1">
997 <command_interface name="position"/>
998 <state_interface name="position"/>
999 <state_interface name="velocity"/>
1000 <command_interface name="max_velocity" />
1003 <ros2_control name="TestSensorHardware" type="sensor">
1005 <plugin>test_sensor23</plugin>
1006 <param name="example_param_write_for_sec">2</param>
1007 <param name="example_param_read_for_sec">2</param>
1009 <sensor name="sensor1">
1010 <state_interface name="velocity"/>
1013 <ros2_control name="TestSystemHardware" type="system">
1015 <plugin>test_system</plugin>
1016 <param name="example_param_write_for_sec">2</param>
1017 <param name="example_param_read_for_sec">2</param>
1019 <joint name="joint2">
1020 <command_interface name="velocity"/>
1021 <state_interface name="position"/>
1022 <state_interface name="velocity"/>
1023 <state_interface name="acceleration"/>
1024 <command_interface name="max_acceleration" />
1026 <joint name="joint3">
1027 <command_interface name="velocity"/>
1028 <state_interface name="position"/>
1029 <state_interface name="velocity"/>
1030 <state_interface name="acceleration"/>
1032 <gpio name="configuration">
1033 <command_interface name="max_tcp_jerk"/>
1034 <state_interface name="max_tcp_jerk"/>
1038const auto hardware_resources_not_existing_system_plugin =
1040 <ros2_control name="TestActuatorHardware" type="actuator">
1042 <plugin>test_actuator</plugin>
1044 <joint name="joint1">
1045 <command_interface name="position"/>
1046 <state_interface name="position"/>
1047 <state_interface name="velocity"/>
1048 <command_interface name="max_velocity" />
1051 <ros2_control name="TestSensorHardware" type="sensor">
1053 <plugin>test_sensor</plugin>
1054 <param name="example_param_write_for_sec">2</param>
1055 <param name="example_param_read_for_sec">2</param>
1057 <sensor name="sensor1">
1058 <state_interface name="velocity"/>
1061 <ros2_control name="TestSystemHardware" type="system">
1063 <plugin>test_system23</plugin>
1064 <param name="example_param_write_for_sec">2</param>
1065 <param name="example_param_read_for_sec">2</param>
1067 <joint name="joint2">
1068 <command_interface name="velocity"/>
1069 <state_interface name="position"/>
1070 <state_interface name="velocity"/>
1071 <state_interface name="acceleration"/>
1072 <command_interface name="max_acceleration" />
1074 <joint name="joint3">
1075 <command_interface name="velocity"/>
1076 <state_interface name="position"/>
1077 <state_interface name="velocity"/>
1078 <state_interface name="acceleration"/>
1080 <gpio name="configuration">
1081 <command_interface name="max_tcp_jerk"/>
1082 <state_interface name="max_tcp_jerk"/>
1087const auto hardware_resources_duplicated_component =
1089 <ros2_control name="TestActuatorHardware" type="actuator">
1091 <plugin>test_actuator</plugin>
1093 <joint name="joint1">
1094 <command_interface name="position"/>
1095 <state_interface name="position"/>
1096 <state_interface name="velocity"/>
1097 <command_interface name="max_velocity" />
1100 <ros2_control name="TestActuatorHardware" type="sensor">
1102 <plugin>test_sensor</plugin>
1103 <param name="example_param_write_for_sec">2</param>
1104 <param name="example_param_read_for_sec">2</param>
1106 <sensor name="sensor1">
1107 <state_interface name="velocity"/>
1110 <ros2_control name="TestSystemHardware" type="system">
1112 <plugin>test_system</plugin>
1113 <param name="example_param_write_for_sec">2</param>
1114 <param name="example_param_read_for_sec">2</param>
1116 <joint name="joint2">
1117 <command_interface name="velocity"/>
1118 <state_interface name="position"/>
1119 <state_interface name="velocity"/>
1120 <state_interface name="acceleration"/>
1121 <command_interface name="max_acceleration" />
1123 <joint name="joint3">
1124 <command_interface name="velocity"/>
1125 <state_interface name="position"/>
1126 <state_interface name="velocity"/>
1127 <state_interface name="acceleration"/>
1129 <gpio name="configuration">
1130 <command_interface name="max_tcp_jerk"/>
1131 <state_interface name="max_tcp_jerk"/>
1136const auto hardware_resources_actuator_initializaion_error =
1138 <ros2_control name="TestActuatorHardware" type="actuator">
1140 <plugin>test_actuator</plugin>
1142 <joint name="joint1">
1143 <command_interface name="position"/>
1144 <state_interface name="position"/>
1145 <state_interface name="does_not_exist"/>
1148 <ros2_control name="TestSensorHardware" type="sensor">
1150 <plugin>test_sensor</plugin>
1151 <param name="example_param_write_for_sec">2</param>
1152 <param name="example_param_read_for_sec">2</param>
1154 <sensor name="sensor1">
1155 <state_interface name="velocity"/>
1158 <ros2_control name="TestSystemHardware" type="system">
1160 <plugin>test_system</plugin>
1161 <param name="example_param_write_for_sec">2</param>
1162 <param name="example_param_read_for_sec">2</param>
1164 <joint name="joint2">
1165 <command_interface name="velocity"/>
1166 <state_interface name="position"/>
1167 <state_interface name="velocity"/>
1169 <joint name="joint3">
1170 <command_interface name="velocity"/>
1171 <state_interface name="position"/>
1172 <state_interface name="velocity"/>
1177const auto hardware_resources_sensor_initializaion_error =
1179 <ros2_control name="TestActuatorHardware" type="actuator">
1181 <plugin>test_actuator</plugin>
1183 <joint name="joint1">
1184 <command_interface name="position"/>
1185 <state_interface name="position"/>
1186 <state_interface name="velocity"/>
1189 <ros2_control name="TestSensorHardware" type="sensor">
1191 <plugin>test_sensor</plugin>
1192 <param name="example_param_write_for_sec">2</param>
1193 <param name="example_param_read_for_sec">2</param>
1195 <sensor name="sensor1">
1196 <state_interface name="velocity"/>
1197 <state_interface name="does_not_exist"/>
1200 <ros2_control name="TestSystemHardware" type="system">
1202 <plugin>test_system</plugin>
1203 <param name="example_param_write_for_sec">2</param>
1204 <param name="example_param_read_for_sec">2</param>
1206 <joint name="joint2">
1207 <command_interface name="velocity"/>
1208 <state_interface name="position"/>
1209 <state_interface name="velocity"/>
1211 <joint name="joint3">
1212 <command_interface name="velocity"/>
1213 <state_interface name="position"/>
1214 <state_interface name="velocity"/>
1219const auto hardware_resources_system_initializaion_error =
1221 <ros2_control name="TestActuatorHardware" type="actuator">
1223 <plugin>test_actuator</plugin>
1225 <joint name="joint1">
1226 <command_interface name="position"/>
1227 <state_interface name="position"/>
1228 <state_interface name="velocity"/>
1231 <ros2_control name="TestSensorHardware" type="sensor">
1233 <plugin>test_sensor</plugin>
1234 <param name="example_param_write_for_sec">2</param>
1235 <param name="example_param_read_for_sec">2</param>
1237 <sensor name="sensor1">
1238 <state_interface name="velocity"/>
1241 <ros2_control name="TestSystemHardware" type="system">
1243 <plugin>test_system</plugin>
1244 <param name="example_param_write_for_sec">2</param>
1245 <param name="example_param_read_for_sec">2</param>
1247 <joint name="joint2">
1248 <command_interface name="velocity"/>
1249 <state_interface name="position"/>
1250 <state_interface name="does_not_exist"/>
1252 <joint name="joint3">
1253 <command_interface name="velocity"/>
1254 <state_interface name="position"/>
1255 <state_interface name="does_not_exist"/>
1260const auto hardware_resources_missing_state_keys =
1262 <ros2_control name="TestActuatorHardware" type="actuator">
1264 <plugin>test_actuator</plugin>
1266 <joint name="joint1">
1267 <command_interface name="position"/>
1268 <state_interface name="position"/>
1269 <state_interface name="velocity"/>
1270 <state_interface name="does_not_exist"/>
1273 <ros2_control name="TestSensorHardware" type="sensor">
1275 <plugin>test_sensor</plugin>
1276 <param name="example_param_write_for_sec">2</param>
1277 <param name="example_param_read_for_sec">2</param>
1279 <sensor name="sensor1">
1280 <state_interface name="velocity"/>
1281 <state_interface name="does_not_exist"/>
1282 <state_interface name="does_not_exist"/>
1285 <ros2_control name="TestSystemHardware" type="system">
1287 <plugin>test_system</plugin>
1288 <param name="example_param_write_for_sec">2</param>
1289 <param name="example_param_read_for_sec">2</param>
1291 <joint name="joint2">
1292 <command_interface name="velocity"/>
1293 <command_interface name="max_acceleration"/>
1294 <state_interface name="position"/>
1295 <state_interface name="velocity"/>
1297 <joint name="joint3">
1298 <command_interface name="velocity"/>
1299 <state_interface name="position"/>
1300 <state_interface name="velocity"/>
1301 <state_interface name="does_not_exist"/>
1306const auto hardware_resources_missing_command_keys =
1308 <ros2_control name="TestActuatorHardware" type="actuator">
1310 <plugin>test_actuator</plugin>
1312 <joint name="joint1">
1313 <command_interface name="position"/>
1314 <command_interface name="does_not_exist"/>
1315 <state_interface name="position"/>
1316 <state_interface name="velocity"/>
1319 <ros2_control name="TestSensorHardware" type="sensor">
1321 <plugin>test_sensor</plugin>
1322 <param name="example_param_write_for_sec">2</param>
1323 <param name="example_param_read_for_sec">2</param>
1325 <sensor name="sensor1">
1326 <state_interface name="velocity"/>
1329 <ros2_control name="TestSystemHardware" type="system">
1331 <plugin>test_system</plugin>
1332 <param name="example_param_write_for_sec">2</param>
1333 <param name="example_param_read_for_sec">2</param>
1335 <joint name="joint2">
1336 <command_interface name="velocity"/>
1337 <command_interface name="max_acceleration"/>
1338 <state_interface name="position"/>
1339 <state_interface name="velocity"/>
1341 <joint name="joint3">
1342 <command_interface name="velocity"/>
1343 <command_interface name="does_not_exist"/>
1344 <state_interface name="position"/>
1345 <state_interface name="velocity"/>
1350const auto hardware_resources_with_exclusive_interface =
1352 <ros2_control name="TestActuatorHardware1" type="actuator">
1354 <plugin>test_actuator_exclusive_interfaces</plugin>
1356 <joint name="joint1">
1357 <command_interface name="position"/>
1358 <command_interface name="velocity"/>
1359 <command_interface name="effort"/>
1360 <state_interface name="position"/>
1361 <state_interface name="velocity"/>
1362 <state_interface name="effort"/>
1365 <ros2_control name="TestActuatorHardware2" type="actuator">
1367 <plugin>test_actuator_exclusive_interfaces</plugin>
1369 <joint name="joint2">
1370 <command_interface name="position"/>
1371 <command_interface name="velocity"/>
1372 <command_interface name="effort"/>
1373 <state_interface name="position"/>
1374 <state_interface name="velocity"/>
1375 <state_interface name="effort"/>
1378 <ros2_control name="TestActuatorHardware3" type="actuator">
1380 <plugin>test_actuator_exclusive_interfaces</plugin>
1382 <joint name="joint3">
1383 <command_interface name="position"/>
1384 <command_interface name="velocity"/>
1385 <command_interface name="effort"/>
1386 <state_interface name="position"/>
1387 <state_interface name="velocity"/>
1388 <state_interface name="effort"/>
1391 <ros2_control name="TestSensorHardware" type="sensor">
1393 <plugin>test_sensor</plugin>
1394 <param name="example_param_write_for_sec">2</param>
1395 <param name="example_param_read_for_sec">2</param>
1397 <sensor name="sensor1">
1398 <state_interface name="velocity"/>
1403const auto diffbot_urdf =
1405<?xml version="1.0" ?>
1407 <!-- Space btw top of beam and the each joint -->
1408 <!-- Links: inertial,visual,collision -->
1409 <link name="base_link">
1411 <!-- origin is relative -->
1412 <origin rpy="0 0 0" xyz="0 0 0"/>
1414 <inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
1418 <box size="0.5 0.1 0.1"/>
1422 <!-- origin is relative -->
1423 <origin xyz="0 0 0"/>
1425 <box size="0.5 0.1 0.1"/>
1429 <link name="base_footprint">
1432 <sphere radius="0.01"/>
1436 <origin xyz="0 0 0"/>
1438 <sphere radius="0.00000001"/>
1442 <joint name="base_footprint_joint" type="fixed">
1443 <origin rpy="0 0 0" xyz="0 0 0.11"/>
1444 <child link="base_link"/>
1445 <parent link="base_footprint"/>
1447 <link name="wheel_0_link">
1449 <origin rpy="0 0 0" xyz="0 0 0"/>
1451 <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
1454 <origin rpy="0 0 0" xyz="0 0 0"/>
1456 <cylinder length="0.086" radius="0.11"/>
1460 <origin rpy="0 0 0" xyz="0 0 0"/>
1462 <cylinder length="0.086" radius="0.11"/>
1466 <joint name="wheel_left" type="continuous">
1467 <parent link="base_link"/>
1468 <child link="wheel_0_link"/>
1469 <origin rpy="-1.5707963267948966 0 0" xyz="0.3 0 0"/>
1471 <limit effort="100" velocity="1.0"/>
1473 <!-- Transmission is important to link the joints and the controller -->
1474 <transmission name="wheel_left_trans" type="SimpleTransmission">
1475 <actuator name="wheel_left_motor"/>
1476 <joint name="wheel_left"/>
1477 <mechanicalReduction>1</mechanicalReduction>
1478 <motorTorqueConstant>1</motorTorqueConstant>
1480 <gazebo reference="wheel_0_link">
1481 <material>Gazebo/Red</material>
1483 <link name="wheel_1_link">
1485 <origin rpy="0 0 0" xyz="0 0 0"/>
1487 <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
1490 <origin rpy="0 0 0" xyz="0 0 0"/>
1492 <cylinder length="0.086" radius="0.11"/>
1496 <origin rpy="0 0 0" xyz="0 0 0"/>
1498 <cylinder length="0.086" radius="0.11"/>
1502 <joint name="wheel_right" type="continuous">
1503 <parent link="base_link"/>
1504 <child link="wheel_1_link"/>
1505 <origin rpy="-1.5707963267948966 0 0" xyz="-0.2 0 0"/>
1507 <limit effort="100" velocity="1.0"/>
1509 <!-- Transmission is important to link the joints and the controller -->
1510 <transmission name="wheel_right_trans" type="SimpleTransmission">
1511 <actuator name="wheel_right_motor"/>
1512 <joint name="wheel_right"/>
1513 <mechanicalReduction>1</mechanicalReduction>
1514 <motorTorqueConstant>1</motorTorqueConstant>
1516 <gazebo reference="wheel_1_link">
1517 <material>Gazebo/Red</material>
1520 <gazebo reference="base_link">
1521 <material>Gazebo/Green</material>
1523 <gazebo reference="base_footprint">
1524 <material>Gazebo/Purple</material>
1526 <ros2_control name="TestActuatorHardwareLeft" type="actuator">
1528 <plugin>test_actuator</plugin>
1530 <joint name="wheel_left">
1531 <state_interface name="position"/>
1532 <command_interface name="velocity"/>
1533 <state_interface name="velocity"/>
1536 <ros2_control name="TestActuatorHardwareRight" type="actuator">
1538 <plugin>test_actuator</plugin>
1540 <joint name="wheel_right">
1541 <state_interface name="position"/>
1542 <command_interface name="velocity"/>
1543 <state_interface name="velocity"/>
1546 <ros2_control name="TestIMUSensorHardware" type="sensor">
1548 <plugin>test_hardware_components/TestIMUSensor</plugin>
1550 <sensor name="base_imu">
1551 <state_interface name="orientation.x"/>
1552 <state_interface name="orientation.y"/>
1553 <state_interface name="orientation.z"/>
1554 <state_interface name="orientation.w"/>
1555 <state_interface name="angular_velocity.x"/>
1556 <state_interface name="angular_velocity.y"/>
1557 <state_interface name="angular_velocity.z"/>
1558 <state_interface name="linear_acceleration.x"/>
1559 <state_interface name="linear_acceleration.y"/>
1560 <state_interface name="linear_acceleration.z"/>
1566const auto gripper_urdf_head =
1567 R
"(<?xml version="1.0" ?>
1568<robot name="gripper">
1569 <link name="world"/>
1573 <box size="0.5 1 1"/>
1575 <origin xyz="0 0 0.5"/>
1576 <material name="violet">
1577 <color rgba="0.4 0.18 0.57 1.0" />
1582 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1585 <link name="finger_right">
1588 <box size="0.4 0.1 1"/>
1590 <origin xyz="0 0.05 0.5"/>
1591 <material name="grey">
1592 <color rgba="0.2 0.2 0.2 1"/>
1597 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1600 <link name="finger_left">
1603 <box size="0.4 0.1 1"/>
1605 <origin xyz="0 0.05 0.5"/>
1606 <material name="grey">
1607 <color rgba="0.2 0.2 0.2 1"/>
1612 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1615 <joint name="world_to_base" type="fixed">
1616 <origin rpy="0 0 0" xyz="0 0 0"/>
1617 <parent link="world"/>
1618 <child link="base"/>
1620 <joint name="right_finger_joint" type="prismatic">
1622 <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
1623 <parent link="base"/>
1624 <child link="finger_right"/>
1625 <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
1627 <joint name="left_finger_joint" type="prismatic">
1628 <mimic joint="right_finger_joint" multiplier="2" offset="1"/>
1630 <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
1631 <parent link="base"/>
1632 <child link="finger_left"/>
1633 <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
1637const auto gripper_urdf_head_no_mimic =
1638 R
"(<?xml version="1.0" ?>
1639<robot name="gripper">
1640 <link name="world"/>
1644 <box size="0.5 1 1"/>
1646 <origin xyz="0 0 0.5"/>
1647 <material name="violet">
1648 <color rgba="0.4 0.18 0.57 1.0" />
1653 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1656 <link name="finger_right">
1659 <box size="0.4 0.1 1"/>
1661 <origin xyz="0 0.05 0.5"/>
1662 <material name="grey">
1663 <color rgba="0.2 0.2 0.2 1"/>
1668 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1671 <link name="finger_left">
1674 <box size="0.4 0.1 1"/>
1676 <origin xyz="0 0.05 0.5"/>
1677 <material name="grey">
1678 <color rgba="0.2 0.2 0.2 1"/>
1683 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1686 <joint name="world_to_base" type="fixed">
1687 <origin rpy="0 0 0" xyz="0 0 0"/>
1688 <parent link="world"/>
1689 <child link="base"/>
1691 <joint name="right_finger_joint" type="prismatic">
1693 <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
1694 <parent link="base"/>
1695 <child link="finger_right"/>
1696 <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
1698 <joint name="left_finger_joint" type="prismatic">
1700 <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
1701 <parent link="base"/>
1702 <child link="finger_left"/>
1703 <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
1707const auto gripper_urdf_head_unknown_joint =
1708 R
"(<?xml version="1.0" ?>
1709<robot name="gripper">
1710 <link name="world"/>
1714 <box size="0.5 1 1"/>
1716 <origin xyz="0 0 0.5"/>
1717 <material name="violet">
1718 <color rgba="0.4 0.18 0.57 1.0" />
1723 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1726 <link name="finger_right">
1729 <box size="0.4 0.1 1"/>
1731 <origin xyz="0 0.05 0.5"/>
1732 <material name="grey">
1733 <color rgba="0.2 0.2 0.2 1"/>
1738 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1741 <link name="finger_left">
1744 <box size="0.4 0.1 1"/>
1746 <origin xyz="0 0.05 0.5"/>
1747 <material name="grey">
1748 <color rgba="0.2 0.2 0.2 1"/>
1753 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1756 <joint name="world_to_base" type="fixed">
1757 <origin rpy="0 0 0" xyz="0 0 0"/>
1758 <parent link="world"/>
1759 <child link="base"/>
1761 <joint name="right_finger_joint" type="prismatic">
1763 <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
1764 <parent link="base"/>
1765 <child link="finger_right"/>
1766 <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
1768 <joint name="left_finger_joint" type="prismatic">
1769 <mimic joint="middle_finger_joint" multiplier="1" offset="0"/>
1771 <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
1772 <parent link="base"/>
1773 <child link="finger_left"/>
1774 <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
1778const auto gripper_urdf_head_incomplete =
1779 R
"(<?xml version="1.0" ?>
1780<robot name="gripper">
1781 <link name="world"/>
1785 <box size="0.5 1 1"/>
1787 <origin xyz="0 0 0.5"/>
1788 <material name="violet">
1789 <color rgba="0.4 0.18 0.57 1.0" />
1794 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1797 <link name="finger_right">
1800 <box size="0.4 0.1 1"/>
1802 <origin xyz="0 0.05 0.5"/>
1803 <material name="grey">
1804 <color rgba="0.2 0.2 0.2 1"/>
1809 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1812 <joint name="world_to_base" type="fixed">
1813 <origin rpy="0 0 0" xyz="0 0 0"/>
1814 <parent link="world"/>
1815 <child link="base"/>
1817 <joint name="right_finger_joint" type="prismatic">
1819 <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
1820 <parent link="base"/>
1821 <child link="finger_right"/>
1822 <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
1826const auto gripper_urdf_head_invalid_two_root_links =
1827 R
"(<?xml version="1.0" ?>
1828<robot name="gripper">
1829 <link name="world"/>
1833 <box size="0.5 1 1"/>
1835 <origin xyz="0 0 0.5"/>
1836 <material name="violet">
1837 <color rgba="0.4 0.18 0.57 1.0" />
1842 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1845 <link name="finger_right">
1848 <box size="0.4 0.1 1"/>
1850 <origin xyz="0 0.05 0.5"/>
1851 <material name="grey">
1852 <color rgba="0.2 0.2 0.2 1"/>
1857 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1860 <link name="finger_left">
1863 <box size="0.4 0.1 1"/>
1865 <origin xyz="0 0.05 0.5"/>
1866 <material name="grey">
1867 <color rgba="0.2 0.2 0.2 1"/>
1872 <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
1875 <joint name="world_to_base" type="fixed">
1876 <origin rpy="0 0 0" xyz="0 0 0"/>
1877 <parent link="world"/>
1878 <child link="base"/>
1880 <joint name="right_finger_joint" type="prismatic">
1882 <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
1883 <parent link="base"/>
1884 <child link="finger_right"/>
1885 <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
1889const auto gripper_hardware_resources_no_command_if =
1891 <ros2_control name="TestGripper" type="system">
1892 <joint name="right_finger_joint">
1893 <command_interface name="effort"/>
1894 <state_interface name="position"/>
1895 <state_interface name="velocity"/>
1896 <state_interface name="effort"/>
1898 <joint name="left_finger_joint">
1899 <state_interface name="position"/>
1900 <state_interface name="velocity"/>
1905const auto gripper_hardware_resources_mimic_true_no_command_if =
1907 <ros2_control name="TestGripper" type="system">
1908 <joint name="right_finger_joint">
1909 <command_interface name="effort"/>
1910 <state_interface name="position"/>
1911 <state_interface name="velocity"/>
1912 <state_interface name="effort"/>
1914 <joint name="left_finger_joint" mimic="true">
1915 <state_interface name="position"/>
1916 <state_interface name="velocity"/>
1921const auto gripper_hardware_resources_mimic_true_command_if =
1923 <ros2_control name="TestGripper" type="system">
1924 <joint name="right_finger_joint">
1925 <command_interface name="effort"/>
1926 <state_interface name="position"/>
1927 <state_interface name="velocity"/>
1928 <state_interface name="effort"/>
1930 <joint name="left_finger_joint" mimic="true">
1931 <command_interface name="effort"/>
1932 <state_interface name="position"/>
1933 <state_interface name="velocity"/>
1938const auto gripper_hardware_resources_mimic_false_command_if =
1940 <ros2_control name="TestGripper" type="system">
1941 <joint name="right_finger_joint">
1942 <command_interface name="effort"/>
1943 <state_interface name="position"/>
1944 <state_interface name="velocity"/>
1945 <state_interface name="effort"/>
1947 <joint name="left_finger_joint" mimic="false">
1948 <command_interface name="effort"/>
1949 <state_interface name="position"/>
1950 <state_interface name="velocity"/>
1955const auto diff_drive_robot_sdf =
1957<?xml version="1.0" ?>
1959 <model canonical_link="base_link" name="my_robot">
1961 <link name="base_link">
1962 <must_be_base_link>true</must_be_base_link>
1965 <joint name="chassis_joint" type="fixed">
1966 <parent>base_link</parent>
1967 <child>chassis_link</child>
1968 <pose relative_to="base_link">0 0 0.075 0 0 0</pose>
1970 <link name="chassis_link">
1971 <pose relative_to="chassis_joint"/>
1972 <visual name="chassis_link_visual">
1981 <ambient>1 1 1 1</ambient>
1982 <diffuse>1 1 1 1</diffuse>
1985 <collision name="chassis_link_collision">
1997 <ixx>0.0046875</ixx>
2000 <iyy>0.0046875</iyy>
2007 <joint name="left_wheel_joint" type="revolute">
2008 <parent>chassis_link</parent>
2009 <child>left_wheel_link</child>
2010 <pose relative_to="chassis_link">0.09 0.16999999999999998 -0.075 -1.5707963267948966 0 0</pose>
2019 <link name="left_wheel_link">
2020 <pose relative_to="left_wheel_joint"/>
2021 <visual name="left_wheel_link_visual">
2024 <radius>0.05</radius>
2025 <length>0.04</length>
2029 <ambient>0 0 1</ambient>
2030 <diffuse>0 0 1</diffuse>
2033 <collision name="left_wheel_link_collision">
2036 <radius>0.05</radius>
2037 <length>0.04</length>
2044 <ixx>7.583333333333335e-05</ixx>
2047 <iyy>7.583333333333335e-05</iyy>
2049 <izz>0.00012500000000000003</izz>
2053 <!-- RIGHT WHEEL -->
2054 <joint name="right_wheel_joint" type="revolute">
2055 <parent>chassis_link</parent>
2056 <child>right_wheel_link</child>
2057 <pose relative_to="chassis_link">0.09 -0.16999999999999998 -0.075 -1.5707963267948966 0 0</pose>
2060 <!-- limit would not be specified because if the type was continuous -->
2067 <link name="right_wheel_link">
2068 <pose relative_to="right_wheel_joint"/>
2069 <visual name="right_wheel_link_visual">
2072 <radius>0.05</radius>
2073 <length>0.04</length>
2077 <ambient>0 0 1</ambient>
2078 <diffuse>0 0 1</diffuse>
2081 <collision name="right_wheel_link_collision">
2084 <radius>0.05</radius>
2085 <length>0.04</length>
2092 <ixx>7.583333333333335e-05</ixx>
2095 <iyy>7.583333333333335e-05</iyy>
2097 <izz>0.00012500000000000003</izz>
2102 <joint name="caster_joint" type="revolute">
2103 <parent>chassis_link</parent>
2104 <child>caster_link</child>
2105 <pose relative_to="chassis_link">-0.09 0 -0.075 0 0 0</pose>
2114 <link name="caster_link">
2115 <pose relative_to="caster_joint"/>
2116 <visual name="caster_link_visual">
2119 <radius>0.05</radius>
2123 <ambient>0 0 1</ambient>
2124 <diffuse>0 0 1</diffuse>
2127 <collision name="caster_link_collision">
2130 <radius>0.05</radius>
2137 <ixx>0.00010000000000000002</ixx>
2140 <iyy>0.00010000000000000002</iyy>
2142 <izz>0.00010000000000000002</izz>
2146 <ros2_control name="GazeboSimSystem" type="system">
2148 <plugin>gz_ros2_control/GazeboSimSystem</plugin>
2150 <joint name="left_wheel_joint">
2151 <command_interface name="velocity">
2152 <param name="min">-10</param>
2153 <param name="max">10</param>
2154 </command_interface>
2155 <state_interface name="velocity"/>
2156 <state_interface name="position"/>
2158 <joint name="right_wheel_joint">
2159 <command_interface name="velocity">
2160 <param name="min">-10</param>
2161 <param name="max">10</param>
2162 </command_interface>
2163 <state_interface name="velocity"/>
2164 <state_interface name="position"/>
2167 <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
2168 <parameters>/path/to/config.yml</parameters>
2174const auto minimal_robot_urdf =
2175 std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail);
2176const auto minimal_robot_urdf_no_limits = std::string(urdf_head_continuous_missing_limits) +
2177 std::string(hardware_resources) + std::string(urdf_tail);
2178const auto minimal_async_robot_urdf =
2179 std::string(urdf_head) + std::string(async_hardware_resources) + std::string(urdf_tail);
2180const auto minimal_robot_urdf_with_different_hw_rw_rate =
2181 std::string(urdf_head) + std::string(hardware_resources_with_different_rw_rates) +
2182 std::string(urdf_tail);
2183const auto minimal_uninitializable_robot_urdf =
2184 std::string(urdf_head) + std::string(uninitializable_hardware_resources) + std::string(urdf_tail);
2186const auto minimal_robot_not_existing_actuator_plugin =
2187 std::string(urdf_head) + std::string(hardware_resources_not_existing_actuator_plugin) +
2188 std::string(urdf_tail);
2189const auto minimal_robot_not_existing_sensors_plugin =
2190 std::string(urdf_head) + std::string(hardware_resources_not_existing_sensor_plugin) +
2191 std::string(urdf_tail);
2192const auto minimal_robot_not_existing_system_plugin =
2193 std::string(urdf_head) + std::string(hardware_resources_not_existing_system_plugin) +
2194 std::string(urdf_tail);
2196const auto minimal_robot_duplicated_component =
2197 std::string(urdf_head) + std::string(hardware_resources_duplicated_component) +
2198 std::string(urdf_tail);
2200const auto minimal_robot_actuator_initialization_error =
2201 std::string(urdf_head) + std::string(hardware_resources_not_existing_actuator_plugin) +
2202 std::string(urdf_tail);
2203const auto minimal_robot_sensor_initialization_error =
2204 std::string(urdf_head) + std::string(hardware_resources_sensor_initializaion_error) +
2205 std::string(urdf_tail);
2206const auto minimal_robot_system_initialization_error =
2207 std::string(urdf_head) + std::string(hardware_resources_system_initializaion_error) +
2208 std::string(urdf_tail);
2210const auto minimal_robot_missing_state_keys_urdf =
2211 std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) +
2212 std::string(urdf_tail);
2214const auto minimal_robot_missing_command_keys_urdf =
2215 std::string(urdf_head) + std::string(hardware_resources_missing_command_keys) +
2216 std::string(urdf_tail);
2218[[maybe_unused]]
const std::string TEST_ACTUATOR_HARDWARE_NAME =
"TestActuatorHardware";
2219[[maybe_unused]]
const std::string TEST_ACTUATOR_HARDWARE_TYPE =
"actuator";
2220[[maybe_unused]]
const std::string TEST_ACTUATOR_HARDWARE_PLUGIN_NAME =
"test_actuator";
2221[[maybe_unused]]
const std::vector<std::string> TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES = {
2222 "joint1/position",
"joint1/max_velocity"};
2223[[maybe_unused]]
const std::vector<std::string> TEST_ACTUATOR_HARDWARE_STATE_INTERFACES = {
2224 "joint1/position",
"joint1/velocity",
"joint1/some_unlisted_interface"};
2226[[maybe_unused]]
const std::string TEST_SENSOR_HARDWARE_NAME =
"TestSensorHardware";
2227[[maybe_unused]]
const std::string TEST_SENSOR_HARDWARE_TYPE =
"sensor";
2228[[maybe_unused]]
const std::string TEST_SENSOR_HARDWARE_PLUGIN_NAME =
"test_sensor";
2229[[maybe_unused]]
const std::vector<std::string> TEST_SENSOR_HARDWARE_COMMAND_INTERFACES = {
""};
2230[[maybe_unused]]
const std::vector<std::string> TEST_SENSOR_HARDWARE_STATE_INTERFACES = {
2231 "sensor1/velocity"};
2233[[maybe_unused]]
const std::string TEST_SYSTEM_HARDWARE_NAME =
"TestSystemHardware";
2234[[maybe_unused]]
const std::string TEST_SYSTEM_HARDWARE_TYPE =
"system";
2235[[maybe_unused]]
const std::string TEST_SYSTEM_HARDWARE_PLUGIN_NAME =
"test_system";
2236[[maybe_unused]]
const std::vector<std::string> TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES = {
2237 "joint2/velocity",
"joint2/max_acceleration",
"joint3/velocity",
"configuration/max_tcp_jerk"};
2238[[maybe_unused]]
const std::vector<std::string> TEST_SYSTEM_HARDWARE_STATE_INTERFACES = {
2239 "joint2/position",
"joint2/velocity",
"joint2/acceleration",
"joint3/position",
2240 "joint3/velocity",
"joint3/acceleration",
"configuration/max_tcp_jerk"};