Skip to content

Commit da0426c

Browse files
IshanDutta11mdurrani808a-ciresumdloop-adminduttaishan01
authored
New 3 dof setup (#108)
* configured for a 3dof controller, we can improve base design later * extra fixes before actual testing * Update with real life testing code (#78) * update to latest * update rear ackermann controller * Removed bad paramters * remove todo nodes * Flipped the turning * swap the right and left speed * working on rover * updated athena_drive.launch.py * updated zed launch * maybe fix the zed issue * updated drive urdf * ackermann guarding * teleop config testing * Added steering limits * Revert ZED-related changes from nav_testing_2324 Restores upstream/main versions of: - sensors.launch.py (publish_odom/map defaults, gps_launch) - zed.launch.py (publish_tf/publish_map_tf back to LaunchConfiguration) - zed_macro_sim.urdf.xacro (color + rgb_sensor link/joint) - athena_drive.urdf.xacro (unconditional zed include + original origin) - nav_bringup.launch.py (remove zed_to_base_footprint static tf) * added arm bringup files * bring over arm can id update as well --------- Co-authored-by: Adrian Cires <ciresadrian@hotmail.com> Co-authored-by: UMDLoop Jetson Orin NX <umdloop@gmail.com> * working, but code wise it should not be prismatic, suitable for now * allows the user to deactivate the talons and have a mock hardware replacement for just those joints * Separates arm into 3 subsections, arm, wrist, and end effector due to the individuality and interchangeability of each subsection. This commit has a separate joint by joint controller for arm (composed of base_yaw, shoulder_pitch, and elbow_pitch), 3 wrist controllers (joint by joint for 3dof, 2dof, and an orientation controller for 3DOF), and gripper claw. If we ever decide to add more joints to arm, use a different kind of wrist control scheme, or have a different kind of end effector entirely, the components as well as the code can now be swapped in and out for it. * Realized a lot of controller switcher functionality had to be modified, so it now functions with this arm setup (will require considerations of drive and always active controllers in yaml file). Also made a 2dof and 3dof trajectory controller config. --------- Co-authored-by: Mohammad Durrani <46766905+mdurrani808@users.noreply.github.com> Co-authored-by: Adrian Cires <ciresadrian@hotmail.com> Co-authored-by: UMDLoop Jetson Orin NX <umdloop@gmail.com> Co-authored-by: duttaishan01 <duttaishan01@FIGARO.myguest.virtualbox.org>
1 parent a3f7d7d commit da0426c

50 files changed

Lines changed: 2756 additions & 906 deletions

File tree

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎src/bringup/scripts/controller_switcher.py‎

Lines changed: 114 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,13 @@
11
#!/usr/bin/env python3
2+
import os
23
import rclpy
34
from rclpy.node import Node
45
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
56
from rclpy.executors import MultiThreadedExecutor
67
from rclpy.duration import Duration
8+
from rclpy.parameter import Parameter
79
import threading
10+
from ament_index_python.packages import get_package_share_directory
811

912
from msgs.srv import SetController
1013
from controller_manager_msgs.srv import SwitchController, ListControllers
@@ -13,13 +16,31 @@
1316
class ControllerSwitcher(Node):
1417
def __init__(self):
1518
super().__init__('controller_switcher')
19+
self.declare_parameters(
20+
namespace='',
21+
parameters=[
22+
('subsystem', Parameter.Type.STRING),
23+
('full_arm', Parameter.Type.STRING_ARRAY),
24+
('arm', Parameter.Type.STRING_ARRAY),
25+
('wrist', Parameter.Type.STRING_ARRAY),
26+
('end_effector', Parameter.Type.STRING_ARRAY)
27+
])
28+
self.subsystem = self.get_parameter('subsystem').value
29+
self.full_arm_controllers = []
30+
self.arm_controllers = []
31+
self.wrist_controllers = []
32+
self.end_effector_controllers = []
33+
34+
# Create separate callback groups
35+
self.service_cb_group = ReentrantCallbackGroup()
36+
self.client_cb_group = MutuallyExclusiveCallbackGroup()
1637

1738
# Create separate callback groups
1839
self.service_cb_group = ReentrantCallbackGroup()
1940
self.client_cb_group = MutuallyExclusiveCallbackGroup()
2041

2142
# Controllers to always keep active or ignore
22-
self.always_active = ["joint_state_broadcaster"]
43+
self.always_active = ["joint_state_broadcaster", "motor_status_broadcaster"]
2344
self.ignore_controllers = []
2445

2546
# Lock to prevent concurrent service processing
@@ -104,6 +125,13 @@ def _handle_set_controller(self, request, response):
104125
self.get_logger().info(f"Available controllers: {all_controllers}")
105126
self.get_logger().info(f"Currently active: {active_controllers}")
106127

128+
if (self.subsystem == 'arm'):
129+
param_controllers = self.full_arm_controllers + self.arm_controllers + self.wrist_controllers + self.end_effector_controllers + self.always_active + self.ignore_controllers
130+
if set(all_controllers) != set(param_controllers):
131+
missing_controllers = set(all_controllers) - set(param_controllers)
132+
extra_controllers = set(param_controllers) - set(all_controllers)
133+
self.get_logger().error(f"Controller list from controller_manager does not match expected controllers from parameters! Here are the missing controllers: {missing_controllers} and here are the extra controllers: {extra_controllers}")
134+
107135
# Check if all requested controllers exist (only if not empty)
108136
if requested_controllers:
109137
missing_controllers = []
@@ -118,27 +146,90 @@ def _handle_set_controller(self, request, response):
118146

119147
# Build lists for activation/deactivation
120148
controllers_to_activate = []
121-
122-
# If not empty, add requested controllers that aren't already active
123-
if requested_controllers:
124-
for controller in requested_controllers:
125-
if controller not in active_controllers:
126-
controllers_to_activate.append(controller)
149+
controllers_to_deactivate = []
150+
127151

128152
# Always ensure joint_state_broadcaster is active
129153
if "joint_state_broadcaster" not in active_controllers and "joint_state_broadcaster" not in controllers_to_activate:
130154
controllers_to_activate.append("joint_state_broadcaster")
155+
156+
157+
def controller_handler(controller, subsystem_controllers):
158+
if controller not in active_controllers:
159+
controllers_to_activate.append(controller)
160+
controllers_to_deactivate.extend(
161+
x for x in subsystem_controllers
162+
if x != controller
163+
and x in active_controllers
164+
and x not in self.always_active
165+
and x not in self.ignore_controllers
166+
)
131167

132-
# Deactivate logic:
133-
# - If empty list: deactivate all except always_active and ignored
134-
# - If controllers specified: deactivate all except requested, always_active, and ignored
135-
controllers_to_deactivate = []
136-
for controller in active_controllers:
137-
if controller in self.always_active or controller in self.ignore_controllers:
138-
continue # Skip protected controllers
139-
140-
if not requested_controllers or controller not in requested_controllers:
141-
controllers_to_deactivate.append(controller)
168+
169+
# Arm specific controller switching logic
170+
if self.subsystem == 'arm' and requested_controllers:
171+
self.full_arm_controllers = self.get_parameter('full_arm').value
172+
self.arm_controllers = self.get_parameter('arm').value
173+
self.wrist_controllers = self.get_parameter('wrist').value
174+
self.end_effector_controllers = self.get_parameter('end_effector').value
175+
176+
# Check if any full arm controller is currently active
177+
active_full_arm = [c for c in active_controllers if c in self.full_arm_controllers]
178+
179+
if active_full_arm:
180+
# Option 1: Switching to another full arm controller
181+
if all(c in self.full_arm_controllers for c in requested_controllers):
182+
controllers_to_deactivate.extend(active_full_arm) # Deactivate current active full arm controller(s)
183+
controllers_to_activate.append(requested_controllers[0]) # Assume only one full arm controller can be active at a time
184+
185+
# Option 2: Switching to a subgroup setup (must define one from each type)
186+
elif (any(c in self.arm_controllers for c in requested_controllers) and
187+
any(c in self.wrist_controllers for c in requested_controllers) and
188+
any(c in self.end_effector_controllers for c in requested_controllers)):
189+
190+
controllers_to_deactivate.extend(active_full_arm) # Deactivate current active full arm controller(s)
191+
controllers_to_activate.extend(list(set(requested_controllers)))
192+
193+
# Invalid request: not full arm, not complete subgroup
194+
else:
195+
self.get_logger().error("Requested controllers must be either a full arm controller or one from each arm/wrist/end-effector.")
196+
response.success = False
197+
response.message = "Requested controllers must be either a full arm controller or one from each arm/wrist/end-effector."
198+
return response
199+
200+
else:
201+
for controller in requested_controllers:
202+
if controller in self.always_active or controller in self.ignore_controllers:
203+
continue # Skip protected controllers
204+
# Activates requested controller if it is part of a subsection
205+
# and deactivates the rest in the subsection
206+
if controller in self.arm_controllers:
207+
controller_handler(controller, self.arm_controllers)
208+
elif controller in self.wrist_controllers:
209+
controller_handler(controller, self.wrist_controllers)
210+
elif controller in self.end_effector_controllers:
211+
controller_handler(controller, self.end_effector_controllers)
212+
elif controller in self.full_arm_controllers:
213+
if controller not in active_controllers:
214+
controllers_to_activate.append(controller)
215+
controllers_to_deactivate = [
216+
c for c in active_controllers
217+
if c not in self.always_active and c not in self.ignore_controllers
218+
]
219+
220+
else:
221+
# Default behavior (non-arm or empty request)
222+
# If not empty, add requested controllers that aren't already active
223+
if requested_controllers:
224+
for controller in requested_controllers:
225+
if controller not in active_controllers:
226+
controllers_to_activate.append(controller)
227+
228+
for controller in active_controllers:
229+
if controller in self.always_active or controller in self.ignore_controllers:
230+
continue # Skip protected controllers
231+
if not requested_controllers or controller not in requested_controllers:
232+
controllers_to_deactivate.append(controller)
142233

143234
# Check if already in desired state
144235
if requested_controllers:
@@ -156,6 +247,12 @@ def _handle_set_controller(self, request, response):
156247
self.get_logger().info("Already in desired state")
157248
return response
158249

250+
# Ensure there are no duplicates in activation/deactivation lists
251+
controllers_to_activate = list(set(controllers_to_activate))
252+
controllers_to_deactivate = list(set(controllers_to_deactivate))
253+
254+
print(self.subsystem)
255+
159256
self.get_logger().info(f"Will activate: {controllers_to_activate}")
160257
self.get_logger().info(f"Will deactivate: {controllers_to_deactivate}")
161258

src/description/ros2_control/arm/arm.smc.ros2_control.xacro renamed to src/description/ros2_control/arm/arm.smc_2dof.ros2_control.xacro

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
<?xml version="1.0"?>
22
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
3-
<xacro:macro name="smc_ros2_control" params="name">
3+
<xacro:macro name="smc_2dof_ros2_control" params="name">
44
<ros2_control name="${name}" type="system">
55
<hardware>
66
<plugin>smc_ros2_control/SMCHardwareInterface</plugin>

‎src/description/ros2_control/arm/arm.smc_3dof.ros2_control.xacro‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
<?xml version="1.0"?>
2+
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
3+
<xacro:macro name="smc_3dof_ros2_control" params="name">
4+
<ros2_control name="${name}" type="system">
5+
<hardware>
6+
<plugin>smc_ros2_control/SMCHardwareInterface</plugin>
7+
<param name="update_rate">10</param> <!-- Hz -->
8+
<param name="logger_rate">5</param> <!-- Hz -->
9+
<param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
10+
<param name="can_interface">can0</param>
11+
</hardware>
12+
13+
<joint name="base_yaw">
14+
<param name="node_id">0x145</param>
15+
<param name="gear_ratio">40</param>
16+
<param name="operating_velocity">300</param> <!-- motor dps -->
17+
<command_interface name="position"/>
18+
<command_interface name="velocity"/>
19+
<state_interface name="position"/>
20+
<state_interface name="velocity"/>
21+
<state_interface name="motor_temperature"/>
22+
<state_interface name="torque_current"/>
23+
</joint>
24+
</ros2_control>
25+
</xacro:macro>
26+
</robot>

src/description/ros2_control/arm/arm.talon.ros2_control.xacro renamed to src/description/ros2_control/arm/arm.talon_2dof.ros2_control.xacro

Lines changed: 13 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,23 @@
11
<?xml version="1.0"?>
22
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
3-
<xacro:macro name="talon_ros2_control" params="name">
3+
<xacro:macro name="talon_2dof_ros2_control" params="name deactivate_talon">
44
<ros2_control name="${name}" type="system">
55
<hardware>
6-
<plugin>talon_ros2_control/TALONHardwareInterface</plugin>
7-
<param name="update_rate">10</param> <!-- Hz -->
8-
<param name="logger_rate">5</param> <!-- Hz -->
9-
<param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
10-
<param name="can_interface">can0</param>
6+
<param name="deactivate_talon">${deactivate_talon}</param>
7+
<xacro:if value="${deactivate_talon}">
8+
<plugin>mock_components/GenericSystem</plugin>
9+
</xacro:if>
10+
<xacro:unless value="${deactivate_talon}">
11+
<plugin>talon_ros2_control/TALONHardwareInterface</plugin>
12+
<param name="update_rate">10</param> <!-- Hz -->
13+
<param name="logger_rate">5</param> <!-- Hz -->
14+
<param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
15+
<param name="can_interface">can0</param>
16+
</xacro:unless>
1117
</hardware>
1218

1319
<joint name="gripper_claw">
14-
<param name="node_id">2</param>
20+
<param name="node_id">4</param>
1521
<param name="max_disp">0.06</param> <!-- meters -->
1622
<param name="joint_type">prismatic</param>
1723
<!-- Motor config (Pololu 37D defaults) -->

‎src/description/ros2_control/arm/arm.talon_3dof.ros2_control.xacro‎

Lines changed: 102 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,102 @@
1+
<?xml version="1.0"?>
2+
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
3+
<xacro:macro name="talon_3dof_ros2_control" params="name deactivate_talon">
4+
<ros2_control name="${name}" type="system">
5+
<hardware>
6+
<xacro:if value="${deactivate_talon}">
7+
<plugin>mock_components/GenericSystem</plugin>
8+
</xacro:if>
9+
<xacro:unless value="${deactivate_talon}">
10+
<plugin>talon_ros2_control/TALONHardwareInterface</plugin>
11+
<param name="update_rate">10</param> <!-- Hz -->
12+
<param name="logger_rate">5</param> <!-- Hz -->
13+
<param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
14+
<param name="can_interface">can0</param>
15+
</xacro:unless>
16+
</hardware>
17+
18+
<joint name="wrist_motor_a">
19+
<param name="node_id">6</param>
20+
<param name="max_disp">0.06</param> <!-- meters -->
21+
<param name="joint_type">revolute</param>
22+
<!-- Motor config (Pololu 37D defaults) -->
23+
<param name="encoder_resolution">64.0</param>
24+
<param name="gear_ratio">102.08</param>
25+
<param name="inverted">true</param>
26+
<param name="sensor_phase">true</param>
27+
<param name="kP">0.25</param>
28+
<param name="kI">0.0</param>
29+
<param name="kD">0.0</param>
30+
<param name="kF">0.0</param>
31+
<command_interface name="position"/>
32+
<command_interface name="velocity"/>
33+
<state_interface name="position"/>
34+
<state_interface name="velocity"/>
35+
<state_interface name="motor_temperature"/>
36+
<state_interface name="torque_current"/>
37+
</joint>
38+
39+
<joint name="wrist_motor_b">
40+
<param name="node_id">4</param>
41+
<param name="max_disp">0.06</param> <!-- meters -->
42+
<param name="joint_type">revolute</param>
43+
<!-- Motor config (Pololu 37D defaults) -->
44+
<param name="encoder_resolution">64.0</param>
45+
<param name="gear_ratio">102.08</param>
46+
<param name="inverted">true</param>
47+
<param name="sensor_phase">true</param>
48+
<param name="kP">0.25</param>
49+
<param name="kI">0.0</param>
50+
<param name="kD">0.0</param>
51+
<param name="kF">0.0</param>
52+
<command_interface name="position"/>
53+
<command_interface name="velocity"/>
54+
<state_interface name="position"/>
55+
<state_interface name="velocity"/>
56+
<state_interface name="motor_temperature"/>
57+
<state_interface name="torque_current"/>
58+
</joint>
59+
60+
<joint name="wrist_motor_c">
61+
<param name="node_id">5</param>
62+
<param name="max_disp">0.06</param> <!-- meters -->
63+
<param name="joint_type">revolute</param>
64+
<!-- Motor config (Pololu 37D defaults) -->
65+
<param name="encoder_resolution">64.0</param>
66+
<param name="gear_ratio">102.08</param>
67+
<param name="inverted">true</param>
68+
<param name="sensor_phase">true</param>
69+
<param name="kP">0.25</param>
70+
<param name="kI">0.0</param>
71+
<param name="kD">0.0</param>
72+
<param name="kF">0.0</param>
73+
<command_interface name="position"/>
74+
<command_interface name="velocity"/>
75+
<state_interface name="position"/>
76+
<state_interface name="velocity"/>
77+
<state_interface name="motor_temperature"/>
78+
<state_interface name="torque_current"/>
79+
</joint>
80+
81+
<joint name="gripper_claw">
82+
<param name="node_id">2</param>
83+
<param name="max_disp">0.06</param> <!-- meters -->
84+
<param name="joint_type">prismatic</param>
85+
<!-- Motor config (Pololu 37D defaults) -->
86+
<param name="encoder_resolution">64.0</param>
87+
<param name="gear_ratio">102.08</param>
88+
<param name="distance_per_rev">0.0015</param> <!-- m/rev -->
89+
<param name="inverted">true</param>
90+
<param name="sensor_phase">true</param>
91+
<param name="kP">0.25</param>
92+
<param name="kI">0.0</param>
93+
<param name="kD">0.0</param>
94+
<param name="kF">0.0</param>
95+
<command_interface name="position"/>
96+
<command_interface name="velocity"/>
97+
<state_interface name="position"/>
98+
<state_interface name="velocity"/>
99+
</joint>
100+
</ros2_control>
101+
</xacro:macro>
102+
</robot>

‎src/description/ros2_control/drive/drive.rmd.ros2_control.xacro‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44
<ros2_control name="${name}" type="system">
55
<hardware>
66
<plugin>rmd_ros2_control/RMDHardwareInterface</plugin>
7-
<param name="update_rate">10</param> <!-- Hz -->
7+
<param name="update_rate">30</param> <!-- Hz -->
88
<param name="logger_rate">5</param> <!-- Hz -->
99
<param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
1010
<param name="can_interface">can0</param>

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

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,19 @@
11
<?xml version="1.0"?>
22
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
3-
<xacro:macro name="talon_ros2_control" params="name">
3+
<xacro:macro name="talon_ros2_control" params="name deactivate_talon">
44
<ros2_control name="${name}" type="system">
55
<hardware>
6-
<plugin>talon_ros2_control/TALONHardwareInterface</plugin>
7-
<param name="update_rate">10</param> <!-- Hz -->
8-
<param name="logger_rate">5</param> <!-- Hz -->
9-
<param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
10-
<param name="can_interface">can0</param>
6+
<param name="deactivate_talon">${deactivate_talon}</param>
7+
<xacro:if value="${deactivate_talon}">
8+
<plugin>mock_components/GenericSystem</plugin>
9+
</xacro:if>
10+
<xacro:unless value="${deactivate_talon}">
11+
<plugin>talon_ros2_control/TALONHardwareInterface</plugin>
12+
<param name="update_rate">10</param> <!-- Hz -->
13+
<param name="logger_rate">5</param> <!-- Hz -->
14+
<param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
15+
<param name="can_interface">can0</param>
16+
</xacro:unless>
1117
</hardware>
1218

1319
<joint name="talon_auger">

0 commit comments

Comments
 (0)