Skip to content

Commit cfcebbc

Browse files
2b-t2b-t
authored
Merge pull request #4 from ioio2995/add_timeout_support
feat: Add timeout support
2 parents ce04275 + c078e93 commit cfcebbc

File tree

9 files changed

+48
-9
lines changed

9 files changed

+48
-9
lines changed

‎myactuator_rmd_bringup/launch/myactuator_rmd_control.launch.py

+10-1
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,8 @@ def generate_launch_description():
3434
rqt_joint_trajectory_controller = LaunchConfiguration(rqt_joint_trajectory_controller_parameter_name)
3535
simulation_parameter_name = 'simulation'
3636
simulation = LaunchConfiguration(simulation_parameter_name)
37+
timeout_parameter_name = 'timeout'
38+
timeout = LaunchConfiguration(timeout_parameter_name)
3739
xacro_file_parameter_name = 'xacro_file'
3840
xacro_file = LaunchConfiguration(xacro_file_parameter_name)
3941

@@ -73,6 +75,11 @@ def generate_launch_description():
7375
default_value='true',
7476
description='Simulated or real hardware interface'
7577
)
78+
timeout_cmd = DeclareLaunchArgument(
79+
timeout_parameter_name,
80+
default_value='0',
81+
description='Timeout duration for the actuator operation'
82+
)
7683
default_xacro_file = PathJoinSubstitution(
7784
[
7885
get_package_share_directory('myactuator_rmd_description'),
@@ -92,7 +99,8 @@ def generate_launch_description():
9299
'actuator:=', actuator, ' ',
93100
'simulation:=', simulation, ' ',
94101
'ifname:=', ifname, ' ',
95-
'actuator_id:=', actuator_id
102+
'actuator_id:=', actuator_id, ' ',
103+
'timeout:=', timeout
96104
]
97105
)
98106
robot_description = {'robot_description': robot_description_content}
@@ -187,6 +195,7 @@ def generate_launch_description():
187195
ld.add_action(rqt_controller_manager_cmd)
188196
ld.add_action(rqt_joint_trajectory_controller_cmd)
189197
ld.add_action(simulation_parameter_cmd)
198+
ld.add_action(timeout_cmd)
190199
ld.add_action(xacro_file_parameter_cmd)
191200
ld.add_action(description_launch)
192201
ld.add_action(joint_state_broadcaster_spawner_node)

‎myactuator_rmd_description/launch/myactuator_rmd.launch.py

+10-1
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,8 @@ def generate_launch_description():
2626
ifname = LaunchConfiguration(ifname_parameter_name)
2727
simulation_parameter_name = 'simulation'
2828
simulation = LaunchConfiguration(simulation_parameter_name)
29+
timeout_parameter_name = 'timeout'
30+
timeout = LaunchConfiguration(timeout_parameter_name)
2931
xacro_file_parameter_name = 'xacro_file'
3032
xacro_file = LaunchConfiguration(xacro_file_parameter_name)
3133

@@ -50,6 +52,11 @@ def generate_launch_description():
5052
default_value='true',
5153
description='Simulated or real hardware interface'
5254
)
55+
timeout_cmd = DeclareLaunchArgument(
56+
timeout_parameter_name,
57+
default_value='0',
58+
description='Timeout duration for the actuator operation'
59+
)
5360
default_xacro_file = PathJoinSubstitution(
5461
[
5562
get_package_share_directory('myactuator_rmd_description'),
@@ -69,7 +76,8 @@ def generate_launch_description():
6976
'actuator:=', actuator, ' ',
7077
'simulation:=', simulation, ' ',
7178
'ifname:=', ifname, ' ',
72-
'actuator_id:=', actuator_id
79+
'actuator_id:=', actuator_id, ' ',
80+
'timeout:=', timeout
7381
]
7482
)
7583
robot_description = {'robot_description': robot_description_content}
@@ -85,6 +93,7 @@ def generate_launch_description():
8593
ld.add_action(actuator_cmd)
8694
ld.add_action(actuator_id_cmd)
8795
ld.add_action(ifname_cmd)
96+
ld.add_action(timeout_cmd)
8897
ld.add_action(simulation_parameter_cmd)
8998
ld.add_action(xacro_file_parameter_cmd)
9099
ld.add_action(robot_state_publisher_node)

‎myactuator_rmd_description/urdf/X12_150.xacro

+2-1
Original file line numberDiff line numberDiff line change
@@ -3,11 +3,12 @@
33
<xacro:include filename="$(find myactuator_rmd_description)/urdf/myactuator_rmd.xacro"/>
44

55
<!-- Provide dimensions for specific actuator -->
6-
<xacro:macro name="X12_150" params="prefix joint_name simulation ifname actuator_id">
6+
<xacro:macro name="X12_150" params="prefix joint_name simulation ifname actuator_id timeout">
77
<xacro:myactuator_rmd prefix="${prefix}" joint_name="${joint_name}" simulation="${simulation}"
88
ifname="${ifname}" actuator_id="${actuator_id}"
99
velocity_alpha="0.1" effort_alpha="0.1"
1010
torque_constant="3.33"
11+
timeout="${timeout}"
1112
limit_effort="150.0" limit_lower="${-pi}" limit_upper="${pi}"
1213
limit_velocity="${100/60*2*pi}"
1314
visual="$(find myactuator_rmd_description)/meshes/visual/X12_150/X12_150.dae"

‎myactuator_rmd_description/urdf/X8ProV2.xacro

+2-1
Original file line numberDiff line numberDiff line change
@@ -3,11 +3,12 @@
33
<xacro:include filename="$(find myactuator_rmd_description)/urdf/myactuator_rmd.xacro"/>
44

55
<!-- Provide dimensions for specific actuator -->
6-
<xacro:macro name="X8ProV2" params="prefix joint_name simulation ifname actuator_id">
6+
<xacro:macro name="X8ProV2" params="prefix joint_name simulation ifname actuator_id timeout">
77
<xacro:myactuator_rmd prefix="${prefix}" joint_name="${joint_name}" simulation="${simulation}"
88
ifname="${ifname}" actuator_id="${actuator_id}"
99
velocity_alpha="0.1" effort_alpha="0.1"
1010
torque_constant="2.6"
11+
timeout="${timeout}"
1112
limit_effort="25.0" limit_lower="${-pi}" limit_upper="${pi}"
1213
limit_velocity="${122/60*2*pi}"
1314
visual="$(find myactuator_rmd_description)/meshes/visual/X8ProV2/X8ProV2.dae"

‎myactuator_rmd_description/urdf/myactuator_rmd.ros2_control.xacro

+4-1
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,8 @@
33

44
<!-- ros2_control configuration for a given joint -->
55
<xacro:macro name="myactuator_rmd_ros2_control" params="name joint_name simulation ifname actuator_id
6-
velocity_alpha effort_alpha torque_constant">
6+
velocity_alpha effort_alpha
7+
torque_constant timeout">
78
<ros2_control name="${name}" type="actuator">
89
<hardware>
910
<xacro:if value="${simulation}">
@@ -21,6 +22,8 @@
2122
<param name="velocity_alpha">${velocity_alpha}</param>
2223
<!-- Low-pass filter constant for effort -->
2324
<param name="effort_alpha">${effort_alpha}</param>
25+
<!-- Timeout of the actuator -->
26+
<param name="timeout">${timeout}</param>
2427
</xacro:unless>
2528
</hardware>
2629
<joint name="${joint_name}">

‎myactuator_rmd_description/urdf/myactuator_rmd.xacro

+2-1
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
simulation ifname actuator_id
2121
velocity_alpha:=0.1 effort_alpha:=0.1
2222
torque_constant
23+
timeout
2324
limit_effort limit_lower limit_upper
2425
limit_velocity
2526
visual
@@ -109,7 +110,7 @@
109110
<xacro:myactuator_rmd_ros2_control name="${prefix}${joint_name}_ros2_control" joint_name="${prefix}${joint_name}" simulation="${simulation}"
110111
ifname="${ifname}" actuator_id="${actuator_id}"
111112
velocity_alpha="${velocity_alpha}" effort_alpha="${effort_alpha}"
112-
torque_constant="${torque_constant}"/>
113+
torque_constant="${torque_constant}" timeout="${timeout}"/>
113114
</xacro:macro>
114115

115116
</robot>

‎myactuator_rmd_description/urdf/standalone.urdf.xacro

+6-2
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,8 @@
1010
<xacro:arg name="ifname" default=""/>
1111
<!-- CAN id for real hardware interface -->
1212
<xacro:arg name="actuator_id" default=""/>
13+
<!-- Timeout duration for the actuator operation -->
14+
<xacro:arg name="timeout" default="0"/>
1315

1416
<!-- Define link names -->
1517
<xacro:property name="parent_link" value="world"/>
@@ -22,13 +24,15 @@
2224
<xacro:include filename="$(find myactuator_rmd_description)/urdf/X8ProV2.xacro"/>
2325
<xacro:X8ProV2 prefix="$(arg prefix)" joint_name="${joint_name}"
2426
simulation="$(arg simulation)"
25-
ifname="$(arg ifname)" actuator_id="$(arg actuator_id)"/>
27+
ifname="$(arg ifname)" actuator_id="$(arg actuator_id)"
28+
timeout="$(arg timeout)"/>
2629
</xacro:if>
2730
<xacro:if value="${actuator_model == 'X12_150'}">
2831
<xacro:include filename="$(find myactuator_rmd_description)/urdf/X12_150.xacro"/>
2932
<xacro:X12_150 prefix="$(arg prefix)" joint_name="${joint_name}"
3033
simulation="$(arg simulation)"
31-
ifname="$(arg ifname)" actuator_id="$(arg actuator_id)"/>
34+
ifname="$(arg ifname)" actuator_id="$(arg actuator_id)"
35+
timeout="$(arg timeout)"/>
3236
</xacro:if>
3337

3438
<!-- Parent and child link and their fixed joints to the actuator -->

‎myactuator_rmd_hardware/include/myactuator_rmd_hardware/myactuator_rmd_hardware_interface.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -269,6 +269,7 @@ namespace myactuator_rmd_hardware {
269269
std::uint32_t actuator_id_;
270270
double torque_constant_;
271271
double max_velocity_;
272+
std::chrono::milliseconds timeout_;
272273

273274
// Buffers only used by the main thread
274275
double position_state_;

‎myactuator_rmd_hardware/src/myactuator_rmd_hardware_interface.cpp

+11-1
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,15 @@ namespace myactuator_rmd_hardware {
9090
cycle_time_ = std::chrono::milliseconds(1);
9191
RCLCPP_INFO(getLogger(), "Cycle time not set, defaulting to '%ld' ms.", cycle_time_.count());
9292
}
93-
93+
if (info_.hardware_parameters.find("timeout") != info_.hardware_parameters.end()) {
94+
timeout_= std::chrono::milliseconds(std::stoi(info_.hardware_parameters["timeout"]));
95+
} else {
96+
timeout_ = std::chrono::milliseconds(0);
97+
RCLCPP_INFO(getLogger(), "Timeout not set, defaulting to '%ld' ms", timeout_.count());
98+
}
99+
if (timeout_ == std::chrono::milliseconds(0)) {
100+
RCLCPP_INFO(getLogger(), "Timeout set to 0ms, it will not be used!");
101+
}
94102
driver_ = std::make_unique<myactuator_rmd::CanDriver>(ifname_);
95103
actuator_interface_ = std::make_unique<myactuator_rmd::ActuatorInterface>(*driver_, actuator_id_);
96104
if (!actuator_interface_) {
@@ -345,6 +353,7 @@ namespace myactuator_rmd_hardware {
345353
}
346354

347355
void MyActuatorRmdHardwareInterface::asyncThread(std::chrono::milliseconds const& cycle_time) {
356+
actuator_interface_->setTimeout(timeout_);
348357
while (!stop_async_thread_) {
349358
auto const now {std::chrono::steady_clock::now()};
350359
auto const wakeup_time {now + cycle_time};
@@ -373,6 +382,7 @@ namespace myactuator_rmd_hardware {
373382

374383
std::this_thread::sleep_until(wakeup_time);
375384
}
385+
actuator_interface_->setTimeout(std::chrono::milliseconds(0));
376386
return;
377387
}
378388

0 commit comments

Comments
 (0)