Skip to content

Commit 517d04b

Browse files
dwarakeshbaraneetharandwarakeshbaraneetharan
committed
fix: make motor_status broadcaster resilient and clean up mock/CAN flow
- MotorStatusBroadcaster now auto-discovers motor_temperature/torque_current and no longer fails if joints/interfaces aren’t specified - Move arm mock xacro into ros2_control/arm and add motor telemetry states; apply same cleanup for science and drive mock definitions - Add dev_can_setup.sh and harden CAN setup/teardown behavior - Fix JTC vs manual controller activation to avoid interface clashes
1 parent be2ae0d commit 517d04b

10 files changed

Lines changed: 218 additions & 122 deletions

File tree

src/description/config/mock.ros2_control.xacro renamed to src/description/ros2_control/arm/arm.mock.ros2_control.xacro

Lines changed: 22 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -17,68 +17,68 @@
1717
<joint name="base_yaw">
1818
<command_interface name="position"/>
1919
<command_interface name="velocity"/>
20-
<state_interface name="position">
21-
<param name="initial_value">0.0</param>
22-
</state_interface>
20+
<state_interface name="position"/>
2321
<state_interface name="velocity"/>
22+
<state_interface name="motor_temperature"/>
23+
<state_interface name="torque_current"/>
2424
</joint>
2525

2626
<joint name="shoulder_pitch">
2727
<command_interface name="position"/>
2828
<command_interface name="velocity"/>
29-
<state_interface name="position">
30-
<param name="initial_value">0.0</param>
31-
</state_interface>
29+
<state_interface name="position"/>
3230
<state_interface name="velocity"/>
31+
<state_interface name="motor_temperature"/>
32+
<state_interface name="torque_current"/>
3333
</joint>
3434

3535
<joint name="elbow_pitch">
3636
<command_interface name="position"/>
3737
<command_interface name="velocity"/>
38-
<state_interface name="position">
39-
<param name="initial_value">0.0</param>
40-
</state_interface>
38+
<state_interface name="position"/>
4139
<state_interface name="velocity"/>
40+
<state_interface name="motor_temperature"/>
41+
<state_interface name="torque_current"/>
4242
</joint>
4343

4444
<joint name="wrist_pitch">
4545
<command_interface name="position"/>
4646
<command_interface name="velocity"/>
47-
<state_interface name="position">
48-
<param name="initial_value">0.0</param>
49-
</state_interface>
47+
<state_interface name="position"/>
5048
<state_interface name="velocity"/>
49+
<state_interface name="motor_temperature"/>
50+
<state_interface name="torque_current"/>
5151
</joint>
5252

5353
<joint name="wrist_roll">
5454
<command_interface name="position"/>
5555
<command_interface name="velocity"/>
56-
<state_interface name="position">
57-
<param name="initial_value">0.0</param>
58-
</state_interface>
56+
<state_interface name="position"/>
5957
<state_interface name="velocity"/>
58+
<state_interface name="motor_temperature"/>
59+
<state_interface name="torque_current"/>
6060
</joint>
6161

6262
<joint name="gripper_claw">
6363
<command_interface name="position"/>
6464
<command_interface name="velocity"/>
65-
<state_interface name="position">
66-
<param name="initial_value">-0.0</param>
67-
</state_interface>
65+
<state_interface name="position"/>
6866
<state_interface name="velocity"/>
67+
<state_interface name="motor_temperature"/>
68+
<state_interface name="torque_current"/>
6969
</joint>
7070

7171
<joint name="actuator">
7272
<command_interface name="position"/>
7373
<command_interface name="velocity"/>
74-
<state_interface name="position">
75-
<param name="initial_value">0.0</param>
76-
</state_interface>
74+
<state_interface name="position"/>
7775
<state_interface name="velocity"/>
76+
<state_interface name="motor_temperature"/>
77+
<state_interface name="torque_current"/>
7878
</joint>
7979

8080
</ros2_control>
8181

8282
</xacro:macro>
8383

84-
</robot>
84+
</robot>

‎src/description/ros2_control/science/science.mock.ros2_control.xacro‎

Lines changed: 20 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -14,19 +14,22 @@
1414
<param name="position_state_following_offset">0.0</param>
1515
</hardware>
1616

17-
1817
<joint name="stepper_motor_a">
1918
<command_interface name="position"/>
2019
<command_interface name="velocity"/>
2120
<state_interface name="position"/>
2221
<state_interface name="velocity"/>
22+
<state_interface name="motor_temperature"/>
23+
<state_interface name="torque_current"/>
2324
</joint>
2425

2526
<joint name="stepper_motor_b">
2627
<command_interface name="position"/>
2728
<command_interface name="velocity"/>
2829
<state_interface name="position"/>
2930
<state_interface name="velocity"/>
31+
<state_interface name="motor_temperature"/>
32+
<state_interface name="torque_current"/>
3033
</joint>
3134

3235
<joint name="talon_lift">
@@ -36,10 +39,10 @@
3639
<param name="max">1</param>
3740
</command_interface>
3841
<command_interface name="velocity"/>
39-
<state_interface name="position">
40-
<param name="initial_value">0.0</param>
41-
</state_interface>
42+
<state_interface name="position"/>
4243
<state_interface name="velocity"/>
44+
<state_interface name="motor_temperature"/>
45+
<state_interface name="torque_current"/>
4346
</joint>
4447

4548
<joint name="talon_scoop">
@@ -49,41 +52,51 @@
4952
<param name="max">1</param>
5053
</command_interface>
5154
<command_interface name="velocity"/>
52-
<state_interface name="position">
53-
<param name="initial_value">0.0</param>
54-
</state_interface>
55+
<state_interface name="position"/>
5556
<state_interface name="velocity"/>
57+
<state_interface name="motor_temperature"/>
58+
<state_interface name="torque_current"/>
5659
</joint>
5760

5861
<joint name="servo_scoop_a">
5962
<command_interface name="position"/>
6063
<command_interface name="velocity"/>
6164
<state_interface name="position"/>
65+
<state_interface name="motor_temperature"/>
66+
<state_interface name="torque_current"/>
6267
</joint>
6368

6469
<joint name="servo_scoop_b">
6570
<command_interface name="position"/>
6671
<command_interface name="velocity"/>
6772
<state_interface name="position"/>
73+
<state_interface name="motor_temperature"/>
74+
<state_interface name="torque_current"/>
6875
</joint>
6976

7077
<joint name="auger">
7178
<command_interface name="position"/>
7279
<command_interface name="velocity"/>
7380
<state_interface name="position"/>
7481
<state_interface name="velocity"/>
82+
<state_interface name="motor_temperature"/>
83+
<state_interface name="torque_current"/>
7584
</joint>
7685

7786
<joint name="cap">
7887
<command_interface name="position"/>
7988
<command_interface name="velocity"/>
8089
<state_interface name="position"/>
90+
<state_interface name="motor_temperature"/>
91+
<state_interface name="torque_current"/>
8192
</joint>
8293

8394
<joint name="auger_spinner">
8495
<command_interface name="velocity"/>
8596
<state_interface name="position"/>
8697
<state_interface name="velocity"/>
98+
<state_interface name="motor_temperature"/>
99+
<state_interface name="torque_current"/>
87100
</joint>
88101

89102
</ros2_control>

‎src/description/urdf/athena_arm.urdf.xacro‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
<xacro:include filename="$(find description)/urdf/athena_arm_description.urdf.xacro"/>
1616

1717
<!-- Import mock ros2_control description -->
18-
<xacro:include filename="$(find description)/config/mock.ros2_control.xacro" />
18+
<xacro:include filename="$(find description)/ros2_control/arm/arm.mock.ros2_control.xacro" />
1919

2020
<!-- Import smc ros2_control description -->
2121
<xacro:include filename="$(find description)/ros2_control/arm/arm.smc.ros2_control.xacro" />

‎src/description/urdf/athena_drive/athena_drive_macro.ros2_control.xacro‎

Lines changed: 30 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -31,100 +31,100 @@
3131
<param name="min">-1.0471975512</param>
3232
<param name="max">1.0471975512</param>
3333
</command_interface>
34-
<state_interface name="position">
35-
<param name="initial_value">0.0</param>
36-
</state_interface>
34+
<state_interface name="position"/>
3735
<state_interface name="velocity"/>
36+
<state_interface name="motor_temperature"/>
37+
<state_interface name="torque_current"/>
3838
</joint>
3939

4040
<joint name="${prefix}suspension_front_right_joint">
4141
<command_interface name="position">
4242
<param name="min">-1.0471975512</param>
4343
<param name="max">1.0471975512</param>
4444
</command_interface>
45-
<state_interface name="position">
46-
<param name="initial_value">0.0</param>
47-
</state_interface>
45+
<state_interface name="position"/>
4846
<state_interface name="velocity"/>
47+
<state_interface name="motor_temperature"/>
48+
<state_interface name="torque_current"/>
4949
</joint>
5050

5151
<joint name="${prefix}steer_fl_joint">
5252
<command_interface name="position">
5353
<param name="min">-0.78539816339</param>
5454
<param name="max">0.78539816339</param>
5555
</command_interface>
56-
<state_interface name="position">
57-
<param name="initial_value">0.0</param>
58-
</state_interface>
56+
<state_interface name="position"/>
5957
<state_interface name="velocity"/>
58+
<state_interface name="motor_temperature"/>
59+
<state_interface name="torque_current"/>
6060
</joint>
6161

6262
<joint name="${prefix}steer_fr_joint">
6363
<command_interface name="position">
6464
<param name="min">-0.78539816339</param>
6565
<param name="max">0.78539816339</param>
6666
</command_interface>
67-
<state_interface name="position">
68-
<param name="initial_value">0.0</param>
69-
</state_interface>
67+
<state_interface name="position"/>
7068
<state_interface name="velocity"/>
69+
<state_interface name="motor_temperature"/>
70+
<state_interface name="torque_current"/>
7171
</joint>
7272

7373
<joint name="${prefix}steer_br_joint">
7474
<command_interface name="position">
7575
<param name="min">0.78539816339</param>
7676
<param name="max">-0.78539816339</param>
7777
</command_interface>
78-
<state_interface name="position">
79-
<param name="initial_value">0.0</param>
80-
</state_interface>
78+
<state_interface name="position"/>
8179
<state_interface name="velocity"/>
80+
<state_interface name="motor_temperature"/>
81+
<state_interface name="torque_current"/>
8282
</joint>
8383

8484
<joint name="${prefix}steer_bl_joint">
8585
<command_interface name="position">
8686
<param name="min">-0.78539816339</param>
8787
<param name="max">0.78539816339</param>
8888
</command_interface>
89-
<state_interface name="position">
90-
<param name="initial_value">0.0</param>
91-
</state_interface>
89+
<state_interface name="position"/>
9290
<state_interface name="velocity"/>
91+
<state_interface name="motor_temperature"/>
92+
<state_interface name="torque_current"/>
9393
</joint>
9494

9595

9696
<joint name="${prefix}propulsion_fl_joint">
9797
<command_interface name="velocity"/>
98-
<state_interface name="position">
99-
<param name="initial_value">0.0</param>
100-
</state_interface>
98+
<state_interface name="position"/>
10199
<state_interface name="velocity"/>
100+
<state_interface name="motor_temperature"/>
101+
<state_interface name="torque_current"/>
102102
</joint>
103103

104104
<joint name="${prefix}propulsion_fr_joint">
105105
<command_interface name="velocity"/>
106-
<state_interface name="position">
107-
<param name="initial_value">0.0</param>
108-
</state_interface>
106+
<state_interface name="position"/>
109107
<state_interface name="velocity"/>
108+
<state_interface name="motor_temperature"/>
109+
<state_interface name="torque_current"/>
110110
</joint>
111111

112112

113113
<joint name="${prefix}propulsion_br_joint">
114114
<command_interface name="velocity"/>
115-
<state_interface name="position">
116-
<param name="initial_value">0.0</param>
117-
</state_interface>
115+
<state_interface name="position"/>
118116
<state_interface name="velocity"/>
117+
<state_interface name="motor_temperature"/>
118+
<state_interface name="torque_current"/>
119119
</joint>
120120

121121

122122
<joint name="${prefix}propulsion_bl_joint">
123123
<command_interface name="velocity"/>
124-
<state_interface name="position">
125-
<param name="initial_value">0.0</param>
126-
</state_interface>
124+
<state_interface name="position"/>
127125
<state_interface name="velocity"/>
126+
<state_interface name="motor_temperature"/>
127+
<state_interface name="torque_current"/>
128128
</joint>
129129
</ros2_control>
130130

0 commit comments

Comments
 (0)