|
6 | 6 | prefix
|
7 | 7 | sim_ignition:=false
|
8 | 8 | sim_isaac:=false
|
9 |
| - use_fake_hardware:=false |
10 |
| - fake_sensor_commands:=false |
| 9 | + use_mock_hardware:=false |
| 10 | + mock_sensor_commands:=false |
11 | 11 | com_port:=/dev/ttyUSB0">
|
12 | 12 |
|
13 | 13 | <ros2_control name="${name}" type="system">
|
|
21 | 21 | <xacro:if value="${sim_ignition}">
|
22 | 22 | <plugin>ign_ros2_control/IgnitionSystem</plugin>
|
23 | 23 | </xacro:if>
|
24 |
| - <xacro:if value="${use_fake_hardware}"> |
| 24 | + <xacro:if value="${use_mock_hardware}"> |
25 | 25 | <plugin>mock_components/GenericSystem</plugin>
|
26 |
| - <param name="fake_sensor_commands">${fake_sensor_commands}</param> |
| 26 | + <param name="mock_sensor_commands">${mock_sensor_commands}</param> |
27 | 27 | <param name="state_following_offset">0.0</param>
|
28 | 28 | </xacro:if>
|
29 |
| - <xacro:unless value="${use_fake_hardware or sim_ignition or sim_isaac}"> |
| 29 | + <xacro:unless value="${use_mock_hardware or sim_ignition or sim_isaac}"> |
30 | 30 | <plugin>robotiq_driver/RobotiqGripperHardwareInterface</plugin>
|
31 | 31 | <param name="gripper_closed_position">0.7929</param>
|
32 | 32 | <param name="COM_port">${com_port}</param>
|
|
42 | 42 | <state_interface name="position">
|
43 | 43 | <param name="initial_value">0.7929</param>
|
44 | 44 | </state_interface>
|
45 |
| - <state_interface name="velocity"/> |
| 45 | + <state_interface name="velocity"> |
| 46 | + <param name="initial_value">0.0</param> |
| 47 | + </state_interface> |
46 | 48 | </joint>
|
47 | 49 | <!-- When simulating we need to include the rest of the gripper joints -->
|
48 |
| - <xacro:if value="${use_fake_hardware or sim_isaac or sim_ignition}"> |
| 50 | + <xacro:if value="${use_mock_hardware or sim_isaac or sim_ignition}"> |
49 | 51 | <joint name="${prefix}robotiq_85_right_knuckle_joint">
|
50 | 52 | <param name="mimic">${prefix}robotiq_85_left_knuckle_joint</param>
|
51 | 53 | <param name="multiplier">-1</param>
|
52 | 54 | <xacro:unless value="${sim_ignition}">
|
53 | 55 | <command_interface name="position"/>
|
54 |
| - <state_interface name="position"/> |
55 |
| - <state_interface name="velocity"/> |
| 56 | + <state_interface name="position"> |
| 57 | + <param name="initial_value">-0.7929</param> |
| 58 | + </state_interface> |
| 59 | + <state_interface name="velocity"> |
| 60 | + <param name="initial_value">0.0</param> |
| 61 | + </state_interface> |
56 | 62 | </xacro:unless>
|
57 | 63 | </joint>
|
58 | 64 | <joint name="${prefix}robotiq_85_left_inner_knuckle_joint">
|
59 | 65 | <param name="mimic">${prefix}robotiq_85_left_knuckle_joint</param>
|
60 | 66 | <param name="multiplier">1</param>
|
61 | 67 | <xacro:unless value="${sim_ignition}">
|
62 | 68 | <command_interface name="position"/>
|
63 |
| - <state_interface name="position"/> |
64 |
| - <state_interface name="velocity"/> |
| 69 | + <state_interface name="position"> |
| 70 | + <param name="initial_value">0.7929</param> |
| 71 | + </state_interface> |
| 72 | + <state_interface name="velocity"> |
| 73 | + <param name="initial_value">0.0</param> |
| 74 | + </state_interface> |
65 | 75 | </xacro:unless>
|
66 | 76 | </joint>
|
67 | 77 | <joint name="${prefix}robotiq_85_right_inner_knuckle_joint">
|
68 | 78 | <param name="mimic">${prefix}robotiq_85_left_knuckle_joint</param>
|
69 | 79 | <param name="multiplier">-1</param>
|
70 | 80 | <xacro:unless value="${sim_ignition}">
|
71 | 81 | <command_interface name="position"/>
|
72 |
| - <state_interface name="position"/> |
73 |
| - <state_interface name="velocity"/> |
| 82 | + <state_interface name="position"> |
| 83 | + <param name="initial_value">-0.7929</param> |
| 84 | + </state_interface> |
| 85 | + <state_interface name="velocity"> |
| 86 | + <param name="initial_value">0.0</param> |
| 87 | + </state_interface> |
74 | 88 | </xacro:unless>
|
75 | 89 | </joint>
|
76 | 90 | <joint name="${prefix}robotiq_85_left_finger_tip_joint">
|
77 | 91 | <param name="mimic">${prefix}robotiq_85_left_knuckle_joint</param>
|
78 | 92 | <param name="multiplier">-1</param>
|
79 | 93 | <xacro:unless value="${sim_ignition}">
|
80 | 94 | <command_interface name="position"/>
|
81 |
| - <state_interface name="position"/> |
82 |
| - <state_interface name="velocity"/> |
| 95 | + <state_interface name="position"> |
| 96 | + <param name="initial_value">-0.7929</param> |
| 97 | + </state_interface> |
| 98 | + <state_interface name="velocity"> |
| 99 | + <param name="initial_value">0.0</param> |
| 100 | + </state_interface> |
83 | 101 | </xacro:unless>
|
84 | 102 | </joint>
|
85 | 103 | <joint name="${prefix}robotiq_85_right_finger_tip_joint">
|
86 | 104 | <param name="mimic">${prefix}robotiq_85_left_knuckle_joint</param>
|
87 | 105 | <param name="multiplier">1</param>
|
88 | 106 | <xacro:unless value="${sim_ignition}">
|
89 | 107 | <command_interface name="position"/>
|
90 |
| - <state_interface name="position"/> |
91 |
| - <state_interface name="velocity"/> |
| 108 | + <state_interface name="position"> |
| 109 | + <param name="initial_value">0.7929</param> |
| 110 | + </state_interface> |
| 111 | + <state_interface name="velocity"> |
| 112 | + <param name="initial_value">0.0</param> |
| 113 | + </state_interface> |
92 | 114 | </xacro:unless>
|
93 | 115 | </joint>
|
94 | 116 | </xacro:if>
|
95 | 117 |
|
96 |
| - <!-- Only add this with fake hardware mode --> |
| 118 | + <!-- Only add this with mock hardware mode --> |
97 | 119 | <xacro:unless value="${sim_ignition or sim_isaac}">
|
98 | 120 | <gpio name="reactivate_gripper">
|
99 | 121 | <command_interface name="reactivate_gripper_cmd" />
|
|
0 commit comments