Skip to content

Commit 5c63578

Browse files
VanjoseluisVanjoseluis
authored
Add initial_value checks to state interface tests (#765)
1 parent dde0790 commit 5c63578

9 files changed

Lines changed: 475 additions & 48 deletions

File tree

‎gz_ros2_control_demos/launch/pendulum_example_effort.launch.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@
2626
def generate_launch_description():
2727
# Launch Arguments
2828
use_sim_time = LaunchConfiguration('use_sim_time', default=True)
29-
29+
gz_args = LaunchConfiguration('gz_args', default='')
3030
# Get URDF via xacro
3131
robot_description_content = Command(
3232
[
@@ -92,7 +92,7 @@ def generate_launch_description():
9292
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
9393
'launch',
9494
'gz_sim.launch.py'])]),
95-
launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]),
95+
launch_arguments=[('gz_args', [gz_args, ' -r -v 1 empty.sdf'])]),
9696
RegisterEventHandler(
9797
event_handler=OnProcessExit(
9898
target_action=gz_spawn_entity,

‎gz_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -92,8 +92,7 @@
9292
</joint>
9393
<joint name="cart_to_pendulum">
9494
<state_interface name="position">
95-
<!-- this does not work if no command interface is set -->
96-
<!-- <param name="initial_value">1.57</param> -->
95+
<param name="initial_value">1.57</param>
9796
</state_interface>
9897
<state_interface name="velocity"/>
9998
<state_interface name="effort"/>

‎gz_ros2_control_tests/tests/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,3 +16,7 @@ add_launch_test(effort_test.py
1616
add_launch_test(ft_sensor_test.py
1717
TIMEOUT 50
1818
)
19+
20+
add_launch_test(pendulum_effort_test.py
21+
TIMEOUT 50
22+
)

‎gz_ros2_control_tests/tests/effort_test.py‎

Lines changed: 58 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -90,17 +90,65 @@ def test_clock(self):
9090
def test_check_if_msgs_published(self):
9191
check_if_js_published(
9292
'/joint_states',
93-
[
94-
'slider_to_cart',
95-
],
93+
['slider_to_cart'],
9694
)
9795

96+
# -------------------------------
97+
# Helper: check initial position
98+
# -------------------------------
99+
def _check_initial_slider_position(self):
100+
from sensor_msgs.msg import JointState
101+
msg = None
102+
103+
def callback(m):
104+
nonlocal msg
105+
msg = m
106+
107+
sub = self.node.create_subscription(
108+
JointState,
109+
'/joint_states',
110+
callback,
111+
10
112+
)
113+
114+
end_time = self.node.get_clock().now().nanoseconds + int(10e9)
115+
while msg is None and self.node.get_clock().now().nanoseconds < end_time:
116+
rclpy.spin_once(self.node, timeout_sec=0.1)
117+
118+
self.node.destroy_subscription(sub)
119+
120+
self.assertIsNotNone(msg, 'No joint_state message received')
121+
self.assertIn('slider_to_cart', msg.name)
122+
123+
joint_idx = msg.name.index('slider_to_cart')
124+
expected_initial_value = 1.0
125+
actual_value = msg.position[joint_idx]
126+
127+
self.assertAlmostEqual(
128+
actual_value,
129+
expected_initial_value,
130+
places=2,
131+
msg=f'Initial position mismatch: expected {expected_initial_value}, got {actual_value}'
132+
)
133+
134+
print(f'Initial value verified: {actual_value} ≈ {expected_initial_value}')
135+
136+
# -------------------------------
137+
# Main test
138+
# -------------------------------
98139
def test_arm(self, launch_service, proc_info, proc_output):
99140

100-
# Check if the controllers are running
101-
cnames = ['joint_trajectory_controller', 'joint_state_broadcaster']
141+
# 1) Check initial position BEFORE any motion
142+
self._check_initial_slider_position()
143+
144+
# 2) Check controllers
145+
cnames = [
146+
'joint_trajectory_controller',
147+
'joint_state_broadcaster',
148+
]
102149
check_controllers_running(self.node, cnames)
103150

151+
# 3) Launch the node that moves the joint
104152
proc_action = Node(
105153
package='gz_ros2_control_demos',
106154
executable='example_effort',
@@ -111,5 +159,8 @@ def test_arm(self, launch_service, proc_info, proc_output):
111159
launch_service, proc_action, proc_info, proc_output
112160
):
113161
proc_info.assertWaitForShutdown(process=proc_action, timeout=300)
114-
launch_testing.asserts.assertExitCodes(proc_info, process=proc_action,
115-
allowable_exit_codes=[0])
162+
launch_testing.asserts.assertExitCodes(
163+
proc_info,
164+
process=proc_action,
165+
allowable_exit_codes=[0]
166+
)

‎gz_ros2_control_tests/tests/ft_sensor_test.py‎

Lines changed: 58 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -90,21 +90,66 @@ def test_clock(self):
9090
def test_check_if_msgs_published(self):
9191
check_if_js_published(
9292
'/joint_states',
93-
[
94-
'slider_to_cart',
95-
],
93+
['slider_to_cart'],
9694
)
9795

96+
# ---------------------------------------------------------
97+
# Helper: check initial slider position BEFORE any motion
98+
# ---------------------------------------------------------
99+
def _check_initial_slider_position(self):
100+
from sensor_msgs.msg import JointState
101+
msg = None
102+
103+
def callback(m):
104+
nonlocal msg
105+
msg = m
106+
107+
sub = self.node.create_subscription(
108+
JointState,
109+
'/joint_states',
110+
callback,
111+
10
112+
)
113+
114+
end_time = self.node.get_clock().now().nanoseconds + int(10e9)
115+
while msg is None and self.node.get_clock().now().nanoseconds < end_time:
116+
rclpy.spin_once(self.node, timeout_sec=0.1)
117+
118+
self.node.destroy_subscription(sub)
119+
120+
self.assertIsNotNone(msg, 'No joint_state message received')
121+
self.assertIn('slider_to_cart', msg.name)
122+
123+
joint_idx = msg.name.index('slider_to_cart')
124+
expected_initial_value = 1.0
125+
actual_value = msg.position[joint_idx]
126+
127+
self.assertAlmostEqual(
128+
actual_value,
129+
expected_initial_value,
130+
places=2,
131+
msg=f'Initial position mismatch: expected {expected_initial_value}, got {actual_value}'
132+
)
133+
134+
print(f'Initial value verified: {actual_value} ≈ {expected_initial_value}')
135+
136+
# ---------------------------------------------------------
137+
# Main test
138+
# ---------------------------------------------------------
98139
def test_arm(self, launch_service, proc_info, proc_output):
99140

100-
# Check if the controllers are running
141+
# 1) Check initial position BEFORE any motion
142+
self._check_initial_slider_position()
143+
144+
# 2) Check controllers
101145
cnames = [
102-
'joint_trajectory_controller',
103-
'joint_state_broadcaster',
104-
'force_torque_sensor_broadcaster'
105-
]
146+
'joint_trajectory_controller',
147+
'joint_state_broadcaster',
148+
'force_torque_sensor_broadcaster'
149+
]
106150
check_controllers_running(self.node, cnames)
107151

152+
# 3) Launch the node that moves the joint
108153
proc_action = Node(
109154
package='gz_ros2_control_demos',
110155
executable='example_position',
@@ -115,5 +160,8 @@ def test_arm(self, launch_service, proc_info, proc_output):
115160
launch_service, proc_action, proc_info, proc_output
116161
):
117162
proc_info.assertWaitForShutdown(process=proc_action, timeout=300)
118-
launch_testing.asserts.assertExitCodes(proc_info, process=proc_action,
119-
allowable_exit_codes=[0])
163+
launch_testing.asserts.assertExitCodes(
164+
proc_info,
165+
process=proc_action,
166+
allowable_exit_codes=[0]
167+
)

‎gz_ros2_control_tests/tests/pendulum_effort_test.py‎

Lines changed: 178 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,178 @@
1+
#!/usr/bin/env python3
2+
# Copyright 2025 ros2_control Maintainers
3+
#
4+
# Licensed under the Apache License, Version 2.0 (the "License");
5+
# you may not use this file except in compliance with the License.
6+
# You may obtain a copy of the License at
7+
#
8+
# http://www.apache.org/licenses/LICENSE-2.0
9+
#
10+
# Unless required by applicable law or agreed to in writing, software
11+
# distributed under the License is distributed on an "AS IS" BASIS,
12+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
# See the License for the specific language governing permissions and
14+
# limitations under the License.
15+
16+
import os
17+
import unittest
18+
19+
from ament_index_python.packages import get_package_share_directory
20+
from controller_manager.test_utils import (
21+
check_controllers_running,
22+
check_if_js_published,
23+
check_node_running
24+
)
25+
from controller_manager_msgs.srv import ListControllers
26+
from launch import LaunchDescription
27+
from launch.actions import IncludeLaunchDescription
28+
from launch.launch_description_sources import PythonLaunchDescriptionSource
29+
from launch_ros.actions import Node
30+
import launch_testing
31+
from launch_testing.actions import ReadyToTest
32+
from launch_testing.util import KeepAliveProc
33+
from launch_testing_ros import WaitForTopics
34+
import psutil
35+
import pytest
36+
import rclpy
37+
from rosgraph_msgs.msg import Clock
38+
39+
40+
# This function specifies the processes to be run for our test
41+
@pytest.mark.rostest
42+
def generate_test_description():
43+
# This is necessary to get unbuffered output from the process under test
44+
proc_env = os.environ.copy()
45+
proc_env['PYTHONUNBUFFERED'] = '1'
46+
launch_include = IncludeLaunchDescription(
47+
PythonLaunchDescriptionSource(
48+
os.path.join(
49+
get_package_share_directory('gz_ros2_control_demos'),
50+
'launch/pendulum_example_effort.launch.py',
51+
)
52+
),
53+
launch_arguments={'gz_args': '--headless-rendering -s'}.items(),
54+
)
55+
56+
return LaunchDescription([launch_include, KeepAliveProc(), ReadyToTest()])
57+
58+
59+
class TestFixture(unittest.TestCase):
60+
61+
@classmethod
62+
def setUpClass(cls):
63+
rclpy.init()
64+
65+
@classmethod
66+
def tearDownClass(cls):
67+
for proc in psutil.process_iter():
68+
# check whether the process name matches
69+
if proc.name() == 'ruby' or 'gz sim' in proc.name():
70+
# up to version 9 of gz-sim
71+
proc.kill()
72+
if 'gz-sim' in proc.name():
73+
# from version 10 of gz-sim
74+
proc.kill()
75+
rclpy.shutdown()
76+
77+
def setUp(self):
78+
self.node = rclpy.create_node('test_node')
79+
80+
def tearDown(self):
81+
self.node.destroy_node()
82+
83+
def _wait_for_controller_manager(self, timeout=10.0):
84+
cli = self.node.create_client(ListControllers, '/controller_manager/list_controllers')
85+
end = self.node.get_clock().now().nanoseconds + int(timeout * 1e9)
86+
87+
while not cli.wait_for_service(timeout_sec=0.1):
88+
if self.node.get_clock().now().nanoseconds > end:
89+
self.fail('controller_manager service not available in time')
90+
91+
def test_node_start(self, proc_output):
92+
check_node_running(self.node, 'robot_state_publisher')
93+
94+
def test_clock(self):
95+
topic_list = [('/clock', Clock)]
96+
with WaitForTopics(topic_list, timeout=10.0):
97+
print('/clock is receiving messages!')
98+
99+
def test_check_if_msgs_published(self):
100+
check_if_js_published(
101+
'/joint_states',
102+
['slider_to_cart', 'cart_to_pendulum'],
103+
)
104+
105+
# ---------------------------------------------------------
106+
# Helper: check initial pendulum angle BEFORE any motion
107+
# ---------------------------------------------------------
108+
def _check_initial_cart_position(self):
109+
from sensor_msgs.msg import JointState
110+
msg = None
111+
112+
def callback(m):
113+
nonlocal msg
114+
msg = m
115+
116+
sub = self.node.create_subscription(
117+
JointState,
118+
'/joint_states',
119+
callback,
120+
10
121+
)
122+
123+
end_time = self.node.get_clock().now().nanoseconds + int(10e9)
124+
while msg is None and self.node.get_clock().now().nanoseconds < end_time:
125+
rclpy.spin_once(self.node, timeout_sec=0.1)
126+
127+
self.node.destroy_subscription(sub)
128+
129+
self.assertIsNotNone(msg, 'No joint_state message received')
130+
self.assertIn('cart_to_pendulum', msg.name)
131+
132+
joint_idx = msg.name.index('cart_to_pendulum')
133+
expected_initial_value = 1.57
134+
actual_value = msg.position[joint_idx]
135+
136+
self.assertAlmostEqual(
137+
actual_value,
138+
expected_initial_value,
139+
places=2,
140+
msg=f'Initial position mismatch: expected {expected_initial_value}, got {actual_value}'
141+
)
142+
143+
print(f'Initial value verified: {actual_value} ≈ {expected_initial_value}')
144+
145+
# ---------------------------------------------------------
146+
# Main test
147+
# ---------------------------------------------------------
148+
def test_arm(self, launch_service, proc_info, proc_output):
149+
150+
# 1) Check initial position BEFORE any motion
151+
self._check_initial_cart_position()
152+
153+
# 2) Wait for controller_manager to be ready
154+
self._wait_for_controller_manager()
155+
156+
# 3) Check controllers
157+
cnames = [
158+
'joint_trajectory_controller',
159+
'joint_state_broadcaster',
160+
]
161+
check_controllers_running(self.node, cnames)
162+
163+
# 4) Launch the node that moves the joint
164+
proc_action = Node(
165+
package='gz_ros2_control_demos',
166+
executable='example_velocity',
167+
output='screen',
168+
)
169+
170+
with launch_testing.tools.launch_process(
171+
launch_service, proc_action, proc_info, proc_output
172+
):
173+
proc_info.assertWaitForShutdown(process=proc_action, timeout=300)
174+
launch_testing.asserts.assertExitCodes(
175+
proc_info,
176+
process=proc_action,
177+
allowable_exit_codes=[0]
178+
)

0 commit comments

Comments
 (0)