@@ -88,19 +88,26 @@ geometry_msgs::msg::PoseStamped generate_random_pose(
88
88
}
89
89
90
90
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 (
91
104
const geometry_msgs::msg::PoseStamped& p,
92
105
xtypes::WritableDynamicDataRef to)
93
106
{
94
107
to[" header" ][" stamp" ][" sec" ] = p.header .stamp .sec ;
95
108
to[" header" ][" stamp" ][" nanosec" ] = p.header .stamp .nanosec ;
96
109
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" ]);
104
111
}
105
112
106
113
xtypes::DynamicData generate_plan_request_msg (
@@ -111,8 +118,8 @@ xtypes::DynamicData generate_plan_request_msg(
111
118
{
112
119
xtypes::DynamicData message (request_type);
113
120
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" ]);
116
123
message[" tolerance" ] = tolerance;
117
124
118
125
return message;
@@ -172,6 +179,59 @@ void compare_plans(
172
179
}
173
180
}
174
181
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
+
175
235
TEST (ROS2, Publish_subscribe_between_ros2_and_mock)
176
236
{
177
237
using namespace std ::chrono_literals;
0 commit comments