Skip to content

Commit db86824

Browse files
committed
Add test checking internal message filtering
Signed-off-by: Jose Antonio Moral <[email protected]>
1 parent 060580f commit db86824

File tree

3 files changed

+88
-9
lines changed

3 files changed

+88
-9
lines changed

ros2/test/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,7 @@ set_property(
9292
TARGET ${PROJECT_NAME}_geometry_msgs ${PROJECT_NAME}_test_domain ${PROJECT_NAME}_primitive_msgs
9393
APPEND PROPERTY COMPILE_DEFINITIONS PRIVATE
9494
"ROS2__GEOMETRY_MSGS__PUBSUB__TEST_CONFIG=\"${CMAKE_CURRENT_LIST_DIR}/resources/ros2__geometry_msgs__pubsub.yaml\""
95+
"ROS2__GEOMETRY_MSGS__LOOPBACK__TEST_CONFIG=\"${CMAKE_CURRENT_LIST_DIR}/resources/ros2__geometry_msgs__loopback.yaml\""
9596
"ROS2__GEOMETRY_MSGS__SERVICES__TEST_CONFIG=\"${CMAKE_CURRENT_LIST_DIR}/resources/ros2__geometry_msgs__services.yaml\""
9697
"ROS2__TEST_DOMAIN__TEST_CONFIG=\"${CMAKE_CURRENT_LIST_DIR}/resources/ros2__domain_change.yaml\""
9798
"ROS2__ROSIDL__BUILD_DIR=\"${CMAKE_BINARY_DIR}/is/rosidl/ros2/lib\""

ros2/test/integration/ros2__geometry_msgs.cpp

Lines changed: 69 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -88,19 +88,26 @@ geometry_msgs::msg::PoseStamped generate_random_pose(
8888
}
8989

9090
void transform_pose_msg(
91+
const geometry_msgs::msg::Pose& pose,
92+
xtypes::WritableDynamicDataRef to)
93+
{
94+
to["position"]["x"] = pose.position.x;
95+
to["position"]["y"] = pose.position.y;
96+
to["position"]["z"] = pose.position.z;
97+
to["orientation"]["x"] = pose.orientation.x;
98+
to["orientation"]["y"] = pose.orientation.y;
99+
to["orientation"]["z"] = pose.orientation.z;
100+
to["orientation"]["w"] = pose.orientation.w;
101+
}
102+
103+
void transform_pose_msg_stamped(
91104
const geometry_msgs::msg::PoseStamped& p,
92105
xtypes::WritableDynamicDataRef to)
93106
{
94107
to["header"]["stamp"]["sec"] = p.header.stamp.sec;
95108
to["header"]["stamp"]["nanosec"] = p.header.stamp.nanosec;
96109
to["header"]["frame_id"] = "map";
97-
to["pose"]["position"]["x"] = p.pose.position.x;
98-
to["pose"]["position"]["y"] = p.pose.position.y;
99-
to["pose"]["position"]["z"] = p.pose.position.z;
100-
to["pose"]["orientation"]["x"] = p.pose.orientation.x;
101-
to["pose"]["orientation"]["y"] = p.pose.orientation.y;
102-
to["pose"]["orientation"]["z"] = p.pose.orientation.z;
103-
to["pose"]["orientation"]["w"] = p.pose.orientation.w;
110+
transform_pose_msg(p.pose, to["pose"]);
104111
}
105112

106113
xtypes::DynamicData generate_plan_request_msg(
@@ -111,8 +118,8 @@ xtypes::DynamicData generate_plan_request_msg(
111118
{
112119
xtypes::DynamicData message(request_type);
113120

114-
transform_pose_msg(goal, message["goal"]);
115-
transform_pose_msg(start, message["start"]);
121+
transform_pose_msg_stamped(goal, message["goal"]);
122+
transform_pose_msg_stamped(start, message["start"]);
116123
message["tolerance"] = tolerance;
117124

118125
return message;
@@ -172,6 +179,59 @@ void compare_plans(
172179
}
173180
}
174181

182+
TEST(ROS2, Filter_internal_messages)
183+
{
184+
using namespace std::chrono_literals;
185+
186+
const double tolerance = 1e-8;
187+
188+
YAML::Node config_node = YAML::LoadFile(ROS2__GEOMETRY_MSGS__LOOPBACK__TEST_CONFIG);
189+
190+
is::core::InstanceHandle handle = is::run_instance(
191+
config_node, {ROS2__ROSIDL__BUILD_DIR});
192+
193+
ASSERT_TRUE(handle);
194+
195+
std::promise<xtypes::DynamicData> msg_promise;
196+
std::future<xtypes::DynamicData> msg_future = msg_promise.get_future();
197+
std::mutex mock_sub_mutex;
198+
bool mock_sub_value_received = false;
199+
auto mock_sub = [&](const xtypes::DynamicData& msg)
200+
{
201+
std::unique_lock<std::mutex> lock(mock_sub_mutex);
202+
if (mock_sub_value_received)
203+
{
204+
return;
205+
}
206+
207+
mock_sub_value_received = true;
208+
msg_promise.set_value(msg);
209+
};
210+
ASSERT_TRUE(is::sh::mock::subscribe("echo_pose", mock_sub));
211+
212+
geometry_msgs::msg::Pose ros2_pose = generate_random_pose().pose;
213+
// Get topic type from ros2 middleware
214+
const is::TypeRegistry& ros2_types = *handle.type_registry("ros2");
215+
const xtypes::DynamicType& topic_type = *ros2_types.at("geometry_msgs/Pose");
216+
xtypes::DynamicData xtypes_pose(topic_type);
217+
transform_pose_msg(ros2_pose, xtypes_pose);
218+
219+
is::sh::mock::publish_message("transmit_pose", xtypes_pose);
220+
221+
// Since Integration Service should block local publications within the SystemHandle,
222+
// the message should never return back from ROS 2.
223+
ASSERT_NE(std::future_status::ready, msg_future.wait_for(5s));
224+
225+
// Quit and wait for no more than a minute. We don't want the test to get
226+
// hung here indefinitely in the case of an error.
227+
handle.quit().wait_for(1min);
228+
229+
// Require that it's no longer running. If it is still running, then it is
230+
// probably stuck, and we should forcefully quit.
231+
ASSERT_TRUE(!handle.running());
232+
ASSERT_TRUE(handle.wait() == 0);
233+
}
234+
175235
TEST(ROS2, Publish_subscribe_between_ros2_and_mock)
176236
{
177237
using namespace std::chrono_literals;
Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
systems:
2+
ros2: { type: ros2 }
3+
mock: { type: mock, types-from: ros2 }
4+
5+
routes:
6+
mock_to_ros2: { from: mock, to: ros2 }
7+
ros2_to_mock: { from: ros2, to: mock }
8+
9+
topics:
10+
transmit_pose:
11+
type: "geometry_msgs/Pose"
12+
route: mock_to_ros2
13+
remap: { ros2: { topic: "mock_to_ros2_to_mock" } }
14+
15+
echo_pose:
16+
type: "geometry_msgs/Pose"
17+
route: ros2_to_mock
18+
remap: { ros2: { topic: "mock_to_ros2_to_mock" } }

0 commit comments

Comments
 (0)