Skip to content

Commit

Permalink
Add option to use mock hardware (#12)
Browse files Browse the repository at this point in the history 
* Add support for mock_components

* Add initial position config file for mock hardware

* Add mock hardware to test matrix

* Remove incorrect dependency

mock_components is part of hardware_interface
  • Loading branch information
@RobertWilbrandt
RobertWilbrandt authored Jan 5, 2025
1 parent 4fccbdd commit b8fb9b4
Show file tree
Hide file tree
Showing 5 changed files with 85 additions and 28 deletions.
6 changes: 6 additions & 0 deletions kuka_rsi_driver/config/initial_positions.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
joint_a1: 0.0
joint_a2: -1.5708
joint_a3: 1.5708
joint_a4: 0.0
joint_a5: 0.0
joint_a6: 0.0
13 changes: 13 additions & 0 deletions kuka_rsi_driver/launch/test_bringup.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,19 @@ def declare_robot_description_arg(name, **kwargs):
"macro_name", description="Name of robot description macro"
)

declare_robot_description_arg(
"use_mock_hardware",
default_value="false",
description="Whether to use mock hardware instead of real hardware interface",
)
declare_robot_description_arg(
"initial_positions_file",
default_value=PathJoinSubstitution(
[kuka_rsi_driver, "config", "initial_positions.yaml"]
),
description="Initial joint positions when using mock hardware",
)

declare_robot_description_arg(
"rsi_listen_ip",
default_value="127.0.0.1",
Expand Down
19 changes: 11 additions & 8 deletions kuka_rsi_driver/test/move_sim.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,17 +56,18 @@

@pytest.mark.launch_test
@launch_testing.parametrize(
"prefix,robot_model",
"prefix,robot_model,use_mock_hardware",
itertools.product(
["", "prefix_"],
[
("kuka_kr5_support", "kr5_arc_macro.xacro", "kuka_kr5_arc"),
("kuka_kr120_support", "kr120r2500pro_macro.xacro", "kuka_kr120r2500pro"),
("kuka_kr210_support", "kr210r3100_macro.xacro", "kuka_kr210r3100"),
],
[True, False],
),
)
def generate_test_description(prefix, robot_model):
def generate_test_description(prefix, robot_model, use_mock_hardware):
kuka_rsi_driver = FindPackageShare("kuka_rsi_driver")

test_description = LaunchDescription()
Expand All @@ -84,6 +85,7 @@ def generate_test_description(prefix, robot_model):
"description_package": robot_model[0],
"description_macro_file": robot_model[1],
"macro_name": robot_model[2],
"use_mock_hardware": f"{use_mock_hardware}",
"launch_rviz": "False",
"prefix": prefix,
}.items(),
Expand All @@ -93,13 +95,14 @@ def generate_test_description(prefix, robot_model):
#
# Start simulator
#
test_description.add_action(
Node(
package="kuka_rsi_driver",
executable="simulator",
arguments=["--host-ip", "127.0.0.1"],
if not use_mock_hardware:
test_description.add_action(
Node(
package="kuka_rsi_driver",
executable="simulator",
arguments=["--host-ip", "127.0.0.1"],
)
)
)

test_description.add_action(ReadyToTest())
return test_description
Expand Down
67 changes: 47 additions & 20 deletions kuka_rsi_driver/urdf/kuka_rsi_driver.ros2_control.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,50 +2,77 @@

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

<xacro:macro name="kuka_rsi_driver_control_joint" params="name">
<xacro:macro name="kuka_rsi_driver_control_joint" params="name initial_position">
<joint name="${name}">
<command_interface name="position" />
<state_interface name="position" />
<state_interface name="position">
<param name="initial_value">${initial_position}</param>
</state_interface>
<state_interface name="effort" />
</joint>
</xacro:macro>

<xacro:macro name="kuka_rsi_driver_control" params="
name
prefix
use_mock_hardware:=false
initial_positions:=${dict(joint_a1=0.0,joint_a2=0.0,joint_a3=0.0,joint_a4=0.0,joint_a5=0.0,joint_a6=0.0)}
rsi_listen_ip
rsi_listen_port:=49152
">
<ros2_control name="${name}" type="system">
<hardware>
<plugin>kuka_rsi_driver/KukaRsiHardwareInterface</plugin>
<param name="listen_address">${rsi_listen_ip}</param>
<param name="listen_port">${rsi_listen_port}</param>
<xacro:if value="${use_mock_hardware}">
<plugin>mock_components/GenericSystem</plugin>
</xacro:if>
<xacro:unless value="${use_mock_hardware}">
<plugin>kuka_rsi_driver/KukaRsiHardwareInterface</plugin>
<param name="listen_address">${rsi_listen_ip}</param>
<param name="listen_port">${rsi_listen_port}</param>
</xacro:unless>
</hardware>

<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a1" />
<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a2" />
<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a3" />
<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a4" />
<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a5" />
<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a6" />
<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a1" initial_position="${initial_positions['joint_a1']}" />
<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a2" initial_position="${initial_positions['joint_a2']}" />
<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a3" initial_position="${initial_positions['joint_a3']}" />
<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a4" initial_position="${initial_positions['joint_a4']}" />
<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a5" initial_position="${initial_positions['joint_a5']}" />
<xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a6" initial_position="${initial_positions['joint_a6']}" />

<sensor name="${prefix}tcp">
<state_interface name="position.x"/>
<state_interface name="position.y"/>
<state_interface name="position.z"/>
<state_interface name="orientation.x"/>
<state_interface name="orientation.y"/>
<state_interface name="orientation.z"/>
<state_interface name="orientation.w"/>
<state_interface name="position.x">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="position.y">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="position.z">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="orientation.x">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="orientation.y">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="orientation.z">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="orientation.w">
<param name="initial_value">1</param>
</state_interface>
</sensor>

<gpio name="${prefix}robot_state">
<state_interface name="program_state" />
<state_interface name="program_state">
<param name="initial_value">3</param>
</state_interface>
</gpio>

<gpio name="${prefix}speed_scaling">
<state_interface name="speed_scaling_factor" />
<state_interface name="speed_scaling_factor">
<param name="initial_value">1</param>
</state_interface>
</gpio>

</ros2_control>
Expand Down
8 changes: 8 additions & 0 deletions kuka_rsi_driver/urdf/test_description.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,11 +7,17 @@
<xacro:arg name="description_macro_file" default="" />
<xacro:arg name="macro_name" default="" />

<xacro:arg name="use_mock_hardware" default="false" />
<xacro:arg name="initial_positions_file" default="$(find kuka_rsi_driver)/config/initial_positions.yaml" />

<xacro:arg name="rsi_listen_ip" default="127.0.0.1" />
<xacro:arg name="rsi_listen_port" default="49152" />

<xacro:arg name="prefix" default="" />

<!-- Convert argument to property so it can be used in a function -->
<xacro:property name="initial_positions_file" default="$(arg initial_positions_file)" />

<!-- Include robot description and invoke macro -->
<xacro:property name="description_package" value="$(arg description_package)" />
<xacro:include filename="$(find ${description_package})/urdf/$(arg description_macro_file)" />
Expand All @@ -22,6 +28,8 @@
<xacro:kuka_rsi_driver_control
name="$(arg macro_name)"
prefix="$(arg prefix)"
use_mock_hardware="$(arg use_mock_hardware)"
initial_positions="${xacro.load_yaml(initial_positions_file)}"
rsi_listen_ip="$(arg rsi_listen_ip)"
rsi_listen_port="$(arg rsi_listen_port)"
/>
Expand Down

0 comments on commit b8fb9b4

Please sign in to comment.