From 47997d90465409c10bedb6a19d6ff76f1c5589a5 Mon Sep 17 00:00:00 2001 From: Cory Crean Date: Fri, 4 Feb 2022 17:44:51 -0500 Subject: [PATCH] Fix race condition in SynchronizedStringParameter::waitForMessage (#1050) Co-authored-by: Tyler Weaver --- .../src/synchronized_string_parameter.cpp | 4 +++- .../rdf_loader/test/test_rdf_integration.cpp | 24 +++++++++++++++++++ 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp index b9796c0600..df05de9a56 100644 --- a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp +++ b/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( + auto const nd_name = std::string(node_->get_name()).append("_ssp_").append(name_); + auto const temp_node = std::make_shared(nd_name); + string_subscriber_ = temp_node->create_subscription( name_, rclcpp::QoS(1).transient_local().reliable(), std::bind(&SynchronizedStringParameter::stringCallback, this, std::placeholders::_1)); diff --git a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp index 00425eaa9b..b1f9ba4bc5 100644 --- a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp +++ b/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("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);