[foxy] Latched Strings for URDF and SRDF (#1414)

moveit_ros/planning/rdf_loader/CMakeLists.txt

@@ -1,6 +1,6 @@
 set(MOVEIT_LIB_NAME moveit_rdf_loader)
 
-add_library(${MOVEIT_LIB_NAME} SHARED src/rdf_loader.cpp)
+add_library(${MOVEIT_LIB_NAME} SHARED src/rdf_loader.cpp src/synchronized_string_parameter.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 ament_target_dependencies(${MOVEIT_LIB_NAME}
       rclcpp
@@ -11,3 +11,30 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
       Boost)
 
 install(DIRECTORY include/ DESTINATION include)
+install(DIRECTORY
+            test/data
+            test/launch
+        DESTINATION share/${PROJECT_NAME}/rdf_loader/test
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ros_testing REQUIRED)
+
+  ament_add_gtest_executable(test_rdf_integration
+      test/test_rdf_integration.cpp
+  )
+  target_link_libraries(test_rdf_integration ${MOVEIT_LIB_NAME})
+  add_ros_test(test/launch/test_rdf_integration.test.py TIMEOUT 120)
+
+  add_executable(boring_string_publisher test/boring_string_publisher.cpp)
+  target_link_libraries(boring_string_publisher ${MOVEIT_LIB_NAME})
+
+  install(
+    TARGETS
+      test_rdf_integration boring_string_publisher
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION lib/${PROJECT_NAME}
+  )
+endif()

moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h

@@ -37,6 +37,7 @@
 #pragma once
 
 #include <moveit/macros/class_forward.h>
+#include <moveit/rdf_loader/synchronized_string_parameter.h>
 #include <urdf/model.h>
 #include <srdfdom/model.h>
 #include <rclcpp/rclcpp.hpp>
@@ -45,24 +46,39 @@ namespace rdf_loader
 {
 MOVEIT_CLASS_FORWARD(RDFLoader);  // Defines RDFLoaderPtr, ConstPtr, WeakPtr... etc
 
+using NewModelCallback = std::function<void()>;
+
 /** @class RDFLoader
- *  @brief Default constructor
- *  @param robot_description The string name corresponding to the ROS param where the URDF is loaded*/
+ */
 class RDFLoader
 {
 public:
   /** @brief Default constructor
-   *  @param robot_description The string name corresponding to the ROS param where the URDF is loaded; the SRDF is
-   * assumed to be at the same param name + the "_semantic" suffix */
-  RDFLoader(const std::shared_ptr<rclcpp::Node>& node, const std::string& robot_description = "robot_description");
+   *
+   *  Loads the URDF from a parameter given by the string argument,
+   *  and the SRDF that has the same name + the "_semantic" suffix
+   *
+   *  If the parameter does not exist, attempt to subscribe to topics
+   *  with the same name and type std_msgs::msg::String.
+   *
+   *  (specifying default_continuous_value/default_timeout allows users
+   *   to specify values without setting ros parameters)
+   *
+   *  @param node ROS interface for parameters / topics
+   *  @param ros_name The string name corresponding to the URDF
+   *  @param default_continuous_value Default value for parameter with "_continuous" suffix.
+   *  @param default_timeout Default value for parameter with "_timeout" suffix.
+   */
+  RDFLoader(const std::shared_ptr<rclcpp::Node>& node, const std::string& ros_name = "robot_description",
+            bool default_continuous_value = false, double default_timeout = 10.0);
 
-  /** \brief Initialize the robot model from a string representation of the URDF and SRDF documents */
+  /** @brief Initialize the robot model from a string representation of the URDF and SRDF documents */
   RDFLoader(const std::string& urdf_string, const std::string& srdf_string);
 
   /** @brief Get the resolved parameter name for the robot description */
   const std::string& getRobotDescription() const
   {
-    return robot_description_;
+    return ros_name_;
   }
 
   /** @brief Get the parsed URDF model*/
@@ -77,6 +93,11 @@ class RDFLoader
     return srdf_;
   }
 
+  void setNewModelCallback(NewModelCallback cb)
+  {
+    new_model_cb_ = cb;
+  }
+
   /** @brief determine if given path points to a xacro file */
   static bool isXacroFile(const std::string& path);
 
@@ -97,7 +118,19 @@ class RDFLoader
                                   const std::string& relative_path, const std::vector<std::string>& xacro_args);
 
 private:
-  std::string robot_description_;
+  bool loadFromStrings();
+
+  void urdfUpdateCallback(const std::string& new_urdf_string);
+  void srdfUpdateCallback(const std::string& new_srdf_string);
+
+  NewModelCallback new_model_cb_;
+
+  std::string ros_name_;
+  std::string urdf_string_, srdf_string_;
+
+  SynchronizedStringParameter urdf_ssp_;
+  SynchronizedStringParameter srdf_ssp_;
+
   srdf::ModelSharedPtr srdf_;
   urdf::ModelInterfaceSharedPtr urdf_;
 };

moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.h

@@ -0,0 +1,84 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, PickNik Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Robotics nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: David V. Lu!! */
+
+#pragma once
+
+#include <std_msgs/msg/string.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace rdf_loader
+{
+using StringCallback = std::function<void(const std::string&)>;
+
+/**
+ * @brief SynchronizedStringParameter is a way to load a string from the ROS environment.
+ *
+ * First it tries to load the string from a parameter.
+ * If that fails, it subscribes to a std_msgs::String topic of the same name to get the value.
+ *
+ * If the parameter is loaded successfully, you can publish the value as a String msg if the publish_NAME param is true.
+ *
+ * You can specify how long to wait for a subscribed message with NAME_timeout (double in seconds)
+ *
+ * By default, the subscription will be killed after the first message is received.
+ * If the parameter NAME_continuous is true, then the parent_callback will be called on every subsequent message.
+ */
+class SynchronizedStringParameter
+{
+public:
+  std::string loadInitialValue(const std::shared_ptr<rclcpp::Node>& node, const std::string& name,
+                               StringCallback parent_callback = {}, bool default_continuous_value = false,
+                               double default_timeout = 10.0);
+
+protected:
+  bool getMainParameter();
+
+  bool shouldPublish();
+
+  bool waitForMessage(const rclcpp::Duration timeout);
+
+  void stringCallback(const std_msgs::msg::String::SharedPtr msg);
+
+  std::shared_ptr<rclcpp::Node> node_;
+  std::string name_;
+  StringCallback parent_callback_;
+
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr string_subscriber_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr string_publisher_;
+
+  std::string content_;
+};
+}  // namespace rdf_loader

moveit_ros/planning/rdf_loader/src/rdf_loader.cpp

@@ -57,80 +57,57 @@ namespace rdf_loader
 {
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_rdf_loader.rdf_loader");
 
-RDFLoader::RDFLoader(const std::shared_ptr<rclcpp::Node>& node, const std::string& robot_description)
-  : robot_description_(robot_description)
+RDFLoader::RDFLoader(const std::shared_ptr<rclcpp::Node>& node, const std::string& ros_name,
+                     bool default_continuous_value, double default_timeout)
+  : ros_name_(ros_name)
 {
-  moveit::tools::Profiler::ScopedStart prof_start;
-  moveit::tools::Profiler::ScopedBlock prof_block("RDFLoader(robot_description)");
-
   auto start = node->now();
 
-  // Check if the robot_description parameter is declared, declare it if it's not declared yet
-  if (!node->has_parameter(robot_description))
-    node->declare_parameter(robot_description);
-  std::string robot_description_content;
-  node->get_parameter_or(robot_description, robot_description_content, std::string());
+  urdf_string_ = urdf_ssp_.loadInitialValue(
+      node, ros_name, [this](const std::string& new_urdf_string) { return urdfUpdateCallback(new_urdf_string); },
+      default_continuous_value, default_timeout);
 
-  if (robot_description_content.empty())
-  {
-    RCLCPP_INFO_ONCE(LOGGER, "Robot model parameter not found! Did you remap '%s'?\n", robot_description.c_str());
-    return;
-  }
+  const std::string srdf_name = ros_name + "_semantic";
+  srdf_string_ = srdf_ssp_.loadInitialValue(
+      node, srdf_name, [this](const std::string& new_srdf_string) { return srdfUpdateCallback(new_srdf_string); },
+      default_continuous_value, default_timeout);
 
-  std::unique_ptr<urdf::Model> urdf(new urdf::Model());
-  if (!urdf->initString(robot_description_content))
+  if (!loadFromStrings())
   {
-    RCLCPP_INFO(LOGGER, "Unable to parse URDF from parameter: '%s'", robot_description.c_str());
     return;
   }
-  urdf_ = std::move(urdf);
-
-  const std::string srdf_description = robot_description + "_semantic";
-  // Check if the robot_description_semantic parameter is declared, declare it if it's not declared yet
-  if (!node->has_parameter(srdf_description))
-    node->declare_parameter(srdf_description);
-  std::string srdf_description_content;
-  node->get_parameter_or(srdf_description, srdf_description_content, std::string());
 
-  if (srdf_description_content.empty())
-  {
-    RCLCPP_INFO_ONCE(LOGGER, "Robot semantic description not found. Did you forget to define or remap '%s'?\n",
-                     srdf_description.c_str());
-    return;
-  }
+  RCLCPP_INFO_STREAM(LOGGER, "Loaded robot model in " << (node->now() - start).seconds() << " seconds");
+}
 
-  srdf::ModelSharedPtr srdf(new srdf::Model());
-  if (!srdf->initString(*urdf_, srdf_description_content))
+RDFLoader::RDFLoader(const std::string& urdf_string, const std::string& srdf_string)
+  : urdf_string_(urdf_string), srdf_string_(srdf_string)
+{
+  if (!loadFromStrings())
   {
-    RCLCPP_ERROR(LOGGER, "Unable to parse SRDF from parameter '%s'", srdf_description.c_str());
     return;
   }
-  srdf_ = std::move(srdf);
-
-  RCLCPP_INFO_STREAM(LOGGER, "Loaded robot model in " << (node->now() - start).seconds() << " seconds");
 }
 
-RDFLoader::RDFLoader(const std::string& urdf_string, const std::string& srdf_string)
+bool RDFLoader::loadFromStrings()
 {
-  moveit::tools::Profiler::ScopedStart prof_start;
-  moveit::tools::Profiler::ScopedBlock prof_block("RDFLoader(string)");
-
-  urdf::Model* umodel = new urdf::Model();
-  urdf_.reset(umodel);
-  if (umodel->initString(urdf_string))
+  std::unique_ptr<urdf::Model> urdf = std::make_unique<urdf::Model>();
+  if (!urdf->initString(urdf_string_))
   {
-    srdf_.reset(new srdf::Model());
-    if (!srdf_->initString(*urdf_, srdf_string))
-    {
-      RCLCPP_ERROR(LOGGER, "Unable to parse SRDF");
-      srdf_.reset();
-    }
+    RCLCPP_INFO(LOGGER, "Unable to parse URDF");
+    return false;
   }
-  else
+
+  srdf::ModelSharedPtr srdf = std::make_shared<srdf::Model>();
+  if (!srdf->initString(*urdf, srdf_string_))
   {
-    RCLCPP_ERROR(LOGGER, "Unable to parse URDF");
-    urdf_.reset();
+    RCLCPP_ERROR(LOGGER, "Unable to parse SRDF");
+    return false;
   }
+
+  urdf_ = std::move(urdf);
+  srdf_ = std::move(srdf);
+  return true;
 }
 
 bool RDFLoader::isXacroFile(const std::string& path)
@@ -175,6 +152,7 @@ bool RDFLoader::loadFileToString(std::string& buffer, const std::string& path)
 bool RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& path,
                                       const std::vector<std::string>& xacro_args)
 {
+  buffer.clear();
   if (path.empty())
   {
     RCLCPP_ERROR(LOGGER, "Path is empty");
@@ -187,7 +165,7 @@ bool RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& pa
     return false;
   }
 
-  std::string cmd = "ros2 run xacro xacro";
+  std::string cmd = "ros2 run xacro xacro ";
   for (const std::string& xacro_arg : xacro_args)
     cmd += xacro_arg + " ";
   cmd += path;
@@ -249,4 +227,30 @@ bool RDFLoader::loadPkgFileToString(std::string& buffer, const std::string& pack
 
   return loadXmlFileToString(buffer, path.string(), xacro_args);
 }
+
+void RDFLoader::urdfUpdateCallback(const std::string& new_urdf_string)
+{
+  urdf_string_ = new_urdf_string;
+  if (!loadFromStrings())
+  {
+    return;
+  }
+  if (new_model_cb_)
+  {
+    new_model_cb_();
+  }
+}
+
+void RDFLoader::srdfUpdateCallback(const std::string& new_srdf_string)
+{
+  srdf_string_ = new_srdf_string;
+  if (!loadFromStrings())
+  {
+    return;
+  }
+  if (new_model_cb_)
+  {
+    new_model_cb_();
+  }
+}
 }  // namespace rdf_loader