Fix race condition in SynchronizedStringParameter::waitForMessage (mo…

…veit#1050)

Co-authored-by: Tyler Weaver <[email protected]>

moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp

@@ -119,7 +119,9 @@ bool SynchronizedStringParameter::shouldPublish()
 
 bool SynchronizedStringParameter::waitForMessage(const rclcpp::Duration timeout)
 {
-  string_subscriber_ = node_->create_subscription<std_msgs::msg::String>(
+  auto const nd_name = std::string(node_->get_name()).append("_ssp_").append(name_);
+  auto const temp_node = std::make_shared<rclcpp::Node>(nd_name);
+  string_subscriber_ = temp_node->create_subscription<std_msgs::msg::String>(
       name_, rclcpp::QoS(1).transient_local().reliable(),
       std::bind(&SynchronizedStringParameter::stringCallback, this, std::placeholders::_1));
 

moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp

@@ -66,6 +66,30 @@ TEST(RDFIntegration, topic_based)
   EXPECT_EQ("gonzo", loader.getSRDF()->getName());
 }
 
+TEST(RDFIntegration, executor)
+{
+  // RDFLoader should successfully load URDF and SRDF strings from a ROS topic when the node that is
+  // passed to it is spinning.
+  // GIVEN a node that has been added to an executor that is spinning on another thread
+  rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("executor");
+
+  // Create a thread to spin an Executor.
+  std::thread([node]() {
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(node);
+    executor.spin();
+  }).detach();
+
+  // WHEN the RDFLoader is created
+  rdf_loader::RDFLoader loader(node, "topic_description");
+
+  // THEN the RDFLoader should return non-null values for the URDF and SRDF model.
+  ASSERT_NE(nullptr, loader.getURDF());
+  EXPECT_EQ("gonzo", loader.getURDF()->name_);
+  ASSERT_NE(nullptr, loader.getSRDF());
+  EXPECT_EQ("gonzo", loader.getSRDF()->getName());
+}
+
 int main(int argc, char** argv)
 {
   rclcpp::init(argc, argv);