Skip to content

Commit f00b6d9

Browse files
delihusdelihus
committed
Added tests for ranges and camera
Signed-off-by: Jakub Delicat <[email protected]>
1 parent 1329f39 commit f00b6d9

11 files changed

+165
-35
lines changed

‎rosbot_description/urdf/components/vl53lox.urdf.xacro

Lines changed: 2 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,9 @@
55

66
<xacro:if value="${namespace == 'None'}">
77
<xacro:property name="namespace_ext" value="" />
8-
<xacro:property name="gz_control_namespace" value="/" />
98
</xacro:if>
109
<xacro:unless value="${namespace == 'None'}">
1110
<xacro:property name="namespace_ext" value="${namespace}/" />
12-
<xacro:property name="gz_control_namespace" value="${namespace}" />
1311
</xacro:unless>
1412

1513
<joint name="${parent}_${prefix}_range_joint" type="fixed">
@@ -26,7 +24,7 @@
2624
<gazebo reference="${prefix}_range">
2725
<sensor name="${namespace_ext}${prefix}_range" type='gpu_lidar'>
2826

29-
<topic>${gz_control_namespace}range/${prefix}</topic>
27+
<topic>${namespace_ext}range/${prefix}</topic>
3028
<frame_id>${prefix}_range</frame_id>
3129
<ignition_frame_id>${prefix}_range</ignition_frame_id>
3230

@@ -55,12 +53,9 @@
5553
<visualize>false</visualize>
5654

5755
<ros>
58-
<namespace></namespace>
56+
<namespace>${namespace_ext}</namespace>
5957
</ros>
6058
</sensor>
61-
<plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
62-
<render_engine>ogre2</render_engine>
63-
</plugin>
6459
</gazebo>
6560
</xacro:if>
6661
</xacro:macro>

‎rosbot_description/urdf/rosbot.urdf.xacro

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -16,20 +16,20 @@
1616
namespace="$(arg namespace)" />
1717

1818
<xacro:include filename="$(find ros_components_description)/urdf/slamtec_rplidar_a2.urdf.xacro" ns="lidar" />
19-
<xacro:lidar.slamtec_rplidar_a2
20-
parent_link="cover_link"
21-
xyz="0.02 0.0 0.0"
22-
rpy="0.0 0.0 0.0"
23-
use_gpu="$(arg use_gpu)"
24-
namespace="$(arg namespace)"
25-
simulation_engine="$(arg simulation_engine)" />
19+
<xacro:lidar.slamtec_rplidar_a2
20+
parent_link="cover_link"
21+
xyz="0.02 0.0 0.0"
22+
rpy="0.0 0.0 0.0"
23+
use_gpu="$(arg use_gpu)"
24+
namespace="$(arg namespace)"
25+
simulation_engine="$(arg simulation_engine)" />
2626

27-
<xacro:include filename="$(find ros_components_description)/urdf/orbbec_astra.urdf.xacro" ns="camera" />
28-
<xacro:camera.orbbec_astra
29-
parent_link="camera_link"
30-
xyz="0.0 0.0 0.0"
31-
rpy="0.0 0.0 0.0"
32-
namespace="$(arg namespace)"
33-
simulation_engine="$(arg simulation_engine)" />
27+
<xacro:include filename="$(find ros_components_description)/urdf/orbbec_astra.urdf.xacro" ns="camera" />
28+
<xacro:camera.orbbec_astra
29+
parent_link="camera_link"
30+
xyz="0.0 0.0 0.0"
31+
rpy="0.0 0.0 0.0"
32+
namespace="$(arg namespace)"
33+
simulation_engine="$(arg simulation_engine)" />
3434

3535
</robot>

‎rosbot_gazebo/launch/spawn.launch.py

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,11 @@
1717
IncludeLaunchDescription,
1818
DeclareLaunchArgument,
1919
)
20-
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, PythonExpression
20+
from launch.substitutions import (
21+
PathJoinSubstitution,
22+
LaunchConfiguration,
23+
PythonExpression,
24+
)
2125
from launch.launch_description_sources import PythonLaunchDescriptionSource
2226
from nav2_common.launch import ReplaceString
2327
from launch_ros.actions import Node, SetParameter

‎rosbot_gazebo/test/test_diff_drive_simulation.py

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -105,11 +105,25 @@ def test_diff_drive_simulation():
105105
assert (
106106
node.controller_odom_flag
107107
), "ROSbot does not rotate properly. Check rosbot_base_controller!"
108-
assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
108+
assert (
109+
node.ekf_odom_flag
110+
), "ROSbot does not rotate properly. Check ekf_filter_node!"
109111

110112
flag = node.scan_event.wait(timeout=20.0)
111113
assert flag, "ROSbot's lidar does not work properly!"
112114

115+
for i in range(len(node.RANGE_SENSORS_TOPICS)):
116+
flag = node.ranges_events[i].wait(timeout=20.0)
117+
assert (
118+
flag
119+
), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
120+
121+
flag = node.camera_color_event.wait(timeout=20.0)
122+
assert flag, "ROSbot's camera color image does not work properly!"
123+
124+
flag = node.camera_points_event.wait(timeout=20.0)
125+
assert flag, "ROSbot's camera point cloud does not work properly!"
126+
113127
finally:
114128
# The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching
115129
# several tests in a row.

‎rosbot_gazebo/test/test_flake8.py

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,4 +20,6 @@
2020
@pytest.mark.linter
2121
def test_flake8():
2222
rc, errors = main_with_errors(argv=[])
23-
assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors)
23+
assert rc == 0, "Found %d code style errors / warnings:\n" % len(
24+
errors
25+
) + "\n".join(errors)

‎rosbot_gazebo/test/test_mecanum_simulation.py

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -118,11 +118,25 @@ def test_mecanum_simulation():
118118
assert (
119119
node.controller_odom_flag
120120
), "ROSbot does not rotate properly. Check rosbot_base_controller!"
121-
assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
121+
assert (
122+
node.ekf_odom_flag
123+
), "ROSbot does not rotate properly. Check ekf_filter_node!"
122124

123125
flag = node.scan_event.wait(timeout=20.0)
124126
assert flag, "ROSbot's lidar does not work properly!"
125127

128+
for i in range(len(node.RANGE_SENSORS_TOPICS)):
129+
flag = node.ranges_events[i].wait(timeout=20.0)
130+
assert (
131+
flag
132+
), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
133+
134+
flag = node.camera_color_event.wait(timeout=20.0)
135+
assert flag, "ROSbot's camera color image does not work properly!"
136+
137+
flag = node.camera_points_event.wait(timeout=20.0)
138+
assert flag, "ROSbot's camera point cloud does not work properly!"
139+
126140
finally:
127141
# The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching
128142
# several tests in a row.

‎rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,9 @@ def test_multirobot_simulation():
7878
nodes.append(node)
7979
executor.add_node(node)
8080

81-
ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,))
81+
ros_spin_thread = Thread(
82+
target=lambda executor: executor.spin(), args=(executor,)
83+
)
8284
ros_spin_thread.start()
8385

8486
for node in nodes:
@@ -116,6 +118,18 @@ def test_multirobot_simulation():
116118
flag = node.scan_event.wait(timeout=20.0)
117119
assert flag, f"{robot_name}'s lidar does not work properly!"
118120

121+
for i in range(len(node.RANGE_SENSORS_TOPICS)):
122+
flag = node.ranges_events[i].wait(timeout=20.0)
123+
assert (
124+
flag
125+
), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
126+
127+
flag = node.camera_color_event.wait(timeout=20.0)
128+
assert flag, "ROSbot's camera color image does not work properly!"
129+
130+
flag = node.camera_points_event.wait(timeout=20.0)
131+
assert flag, "ROSbot's camera point cloud does not work properly!"
132+
119133
node.destroy_node()
120134

121135
finally:

‎rosbot_gazebo/test/test_multirobot_mecanum_simulation.py

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,9 @@ def test_multirobot_mecanum_simulation():
8181
nodes.append(node)
8282
executor.add_node(node)
8383

84-
ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,))
84+
ros_spin_thread = Thread(
85+
target=lambda executor: executor.spin(), args=(executor,)
86+
)
8587
ros_spin_thread.start()
8688

8789
for node in nodes:
@@ -136,6 +138,18 @@ def test_multirobot_mecanum_simulation():
136138
flag = node.scan_event.wait(timeout=20.0)
137139
assert flag, f"{robot_name}'s lidar does not work properly!"
138140

141+
for i in range(len(node.RANGE_SENSORS_TOPICS)):
142+
flag = node.ranges_events[i].wait(timeout=20.0)
143+
assert (
144+
flag
145+
), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
146+
147+
flag = node.camera_color_event.wait(timeout=20.0)
148+
assert flag, "ROSbot's camera color image does not work properly!"
149+
150+
flag = node.camera_points_event.wait(timeout=20.0)
151+
assert flag, "ROSbot's camera point cloud does not work properly!"
152+
139153
node.destroy_node()
140154

141155
finally:

‎rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -101,11 +101,25 @@ def test_namespaced_diff_drive_simulation():
101101
assert (
102102
node.controller_odom_flag
103103
), "ROSbot does not rotate properly. Check rosbot_base_controller!"
104-
assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
104+
assert (
105+
node.ekf_odom_flag
106+
), "ROSbot does not rotate properly. Check ekf_filter_node!"
105107

106108
flag = node.scan_event.wait(timeout=20.0)
107109
assert flag, "ROSbot's lidar does not work properly!"
108110

111+
for i in range(len(node.RANGE_SENSORS_TOPICS)):
112+
flag = node.ranges_events[i].wait(timeout=20.0)
113+
assert (
114+
flag
115+
), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
116+
117+
flag = node.camera_color_event.wait(timeout=20.0)
118+
assert flag, "ROSbot's camera color image does not work properly!"
119+
120+
flag = node.camera_points_event.wait(timeout=20.0)
121+
assert flag, "ROSbot's camera point cloud does not work properly!"
122+
109123
finally:
110124
# The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching
111125
# several tests in a row.

‎rosbot_gazebo/test/test_namespaced_mecanum_simulation.py

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -114,11 +114,25 @@ def test_namespaced_mecanum_simulation():
114114
assert (
115115
node.controller_odom_flag
116116
), "ROSbot does not rotate properly. Check rosbot_base_controller!"
117-
assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
117+
assert (
118+
node.ekf_odom_flag
119+
), "ROSbot does not rotate properly. Check ekf_filter_node!"
118120

119121
flag = node.scan_event.wait(timeout=20.0)
120122
assert flag, "ROSbot's lidar does not work properly!"
121123

124+
for i in range(len(node.RANGE_SENSORS_TOPICS)):
125+
flag = node.ranges_events[i].wait(timeout=20.0)
126+
assert (
127+
flag
128+
), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
129+
130+
flag = node.camera_color_event.wait(timeout=20.0)
131+
assert flag, "ROSbot's camera color image does not work properly!"
132+
133+
flag = node.camera_points_event.wait(timeout=20.0)
134+
assert flag, "ROSbot's camera point cloud does not work properly!"
135+
122136
finally:
123137
# The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching
124138
# several tests in a row.

‎rosbot_gazebo/test/test_utils.py

Lines changed: 51 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@
2222

2323
from geometry_msgs.msg import Twist
2424
from nav_msgs.msg import Odometry
25-
from sensor_msgs.msg import LaserScan
25+
from sensor_msgs.msg import LaserScan, Image, PointCloud2
2626

2727

2828
class SimulationTestNode(Node):
@@ -31,11 +31,16 @@ class SimulationTestNode(Node):
3131
# cause the rosbot_base_controller to determine inaccurate odometry.
3232
ACCURACY = 0.10 # 10% accuracy
3333

34+
RANGE_SENSORS_TOPICS = ["range/fl", "range/fr", "range/rl", "range/rr"]
35+
RANGE_SENSORS_FRAMES = ["fl_range", "fr_range", "rl_range", "rr_range"]
36+
3437
def __init__(self, name="test_node", namespace=None):
3538
super().__init__(name, namespace=namespace)
3639

3740
# Use simulation time to correct run on slow machine
38-
use_sim_time = rclpy.parameter.Parameter("use_sim_time", rclpy.Parameter.Type.BOOL, True)
41+
use_sim_time = rclpy.parameter.Parameter(
42+
"use_sim_time", rclpy.Parameter.Type.BOOL, True
43+
)
3944
self.set_parameters([use_sim_time])
4045

4146
self.VELOCITY_STABILIZATION_DELAY = 3
@@ -51,6 +56,9 @@ def __init__(self, name="test_node", namespace=None):
5156
self.ekf_odom_flag = False
5257
self.odom_tf_event = Event()
5358
self.scan_event = Event()
59+
self.ranges_events = [Event() for _ in range(len(self.RANGE_SENSORS_TOPICS))]
60+
self.camera_color_event = Event()
61+
self.camera_points_event = Event()
5462
self.ros_node_spin_event = Event()
5563

5664
def clear_odom_flag(self):
@@ -76,16 +84,37 @@ def create_test_subscribers_and_publishers(self):
7684
Odometry, "odometry/filtered", self.ekf_callback, 10
7785
)
7886

79-
self.scan_sub = self.create_subscription(LaserScan, "scan", self.scan_callback, 10)
87+
self.scan_sub = self.create_subscription(
88+
LaserScan, "scan", self.scan_callback, 10
89+
)
90+
91+
self.range_subs = []
92+
for range_topic_name in self.RANGE_SENSORS_TOPICS:
93+
sub = self.create_subscription(
94+
LaserScan, range_topic_name, self.ranges_callback, 10
95+
)
96+
self.range_subs.append(sub)
97+
98+
self.camera_color_sub = self.create_subscription(
99+
Image, "camera/image", self.camera_image_callback, 10
100+
)
101+
102+
self.camera_points_sub = self.create_subscription(
103+
PointCloud2, "camera/points", self.camera_points_callback, 10
104+
)
80105

81106
self.timer = self.create_timer(1.0 / 10.0, self.timer_callback)
82107

83108
def start_node_thread(self):
84-
self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,))
109+
self.ros_spin_thread = Thread(
110+
target=lambda node: rclpy.spin(node), args=(self,)
111+
)
85112
self.ros_spin_thread.start()
86113

87114
def is_twist_ok(self, twist: Twist):
88-
def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, eps=0.01):
115+
def are_close_to_each_other(
116+
current_value, dest_value, tolerance=self.ACCURACY, eps=0.01
117+
):
89118
acceptable_range = dest_value * tolerance
90119
return abs(current_value - dest_value) <= acceptable_range + eps
91120

@@ -109,14 +138,30 @@ def timer_callback(self):
109138
self.publish_cmd_vel_messages()
110139

111140
self.current_time = 1e-9 * self.get_clock().now().nanoseconds
112-
if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY:
141+
if (
142+
self.current_time
143+
> self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY
144+
):
113145
self.vel_stabilization_time_event.set()
114146

115147
def scan_callback(self, data: LaserScan):
116148
self.get_logger().debug(f"Received scan length: {len(data.ranges)}")
117149
if data.ranges:
118150
self.scan_event.set()
119151

152+
def ranges_callback(self, data: LaserScan):
153+
index = self.RANGE_SENSORS_FRAMES.index(data.header.frame_id)
154+
if len(data.ranges) == 1:
155+
self.ranges_events[index].set()
156+
157+
def camera_image_callback(self, data: Image):
158+
if data.data:
159+
self.camera_color_event.set()
160+
161+
def camera_points_callback(self, data: PointCloud2):
162+
if data.data:
163+
self.camera_points_event.set()
164+
120165
def publish_cmd_vel_messages(self):
121166
twist_msg = Twist()
122167

0 commit comments

Comments
 (0)