From 4bddaa0f2745faad4443eaddce550be649958483 Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Thu, 28 Jul 2022 14:14:30 -0400 Subject: [PATCH] [foxy] Latched Strings for URDF and SRDF (#1414) --- moveit_ros/planning/rdf_loader/CMakeLists.txt | 29 +++- .../include/moveit/rdf_loader/rdf_loader.h | 49 +++++- .../synchronized_string_parameter.h | 84 ++++++++++ .../planning/rdf_loader/src/rdf_loader.cpp | 114 +++++++------ .../src/synchronized_string_parameter.cpp | 157 ++++++++++++++++++ .../test/boring_string_publisher.cpp | 86 ++++++++++ .../planning/rdf_loader/test/data/gonzo.srdf | 6 + .../planning/rdf_loader/test/data/gonzo.urdf | 40 +++++ .../planning/rdf_loader/test/data/kermit.srdf | 6 + .../planning/rdf_loader/test/data/kermit.urdf | 40 +++++ .../rdf_loader/test/data/robin.srdf.xacro | 9 + .../test/launch/test_rdf_integration.test.py | 78 +++++++++ .../rdf_loader/test/test_rdf_integration.cpp | 110 ++++++++++++ .../robot_state_display.h | 1 + .../src/robot_state_display.cpp | 11 +- 15 files changed, 753 insertions(+), 67 deletions(-) create mode 100644 moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.h create mode 100644 moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp create mode 100644 moveit_ros/planning/rdf_loader/test/boring_string_publisher.cpp create mode 100644 moveit_ros/planning/rdf_loader/test/data/gonzo.srdf create mode 100644 moveit_ros/planning/rdf_loader/test/data/gonzo.urdf create mode 100644 moveit_ros/planning/rdf_loader/test/data/kermit.srdf create mode 100644 moveit_ros/planning/rdf_loader/test/data/kermit.urdf create mode 100644 moveit_ros/planning/rdf_loader/test/data/robin.srdf.xacro create mode 100644 moveit_ros/planning/rdf_loader/test/launch/test_rdf_integration.test.py create mode 100644 moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp diff --git a/moveit_ros/planning/rdf_loader/CMakeLists.txt b/moveit_ros/planning/rdf_loader/CMakeLists.txt index a78045621c..e470ac3552 100644 --- a/moveit_ros/planning/rdf_loader/CMakeLists.txt +++ b/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() diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h index 5764e02b34..60371b8004 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h @@ -37,6 +37,7 @@ #pragma once #include +#include #include #include #include @@ -45,24 +46,39 @@ namespace rdf_loader { MOVEIT_CLASS_FORWARD(RDFLoader); // Defines RDFLoaderPtr, ConstPtr, WeakPtr... etc +using NewModelCallback = std::function; + /** @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& 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& 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& 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_; }; diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.h new file mode 100644 index 0000000000..1af90135c6 --- /dev/null +++ b/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 +#include + +namespace rdf_loader +{ +using StringCallback = std::function; + +/** + * @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& 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 node_; + std::string name_; + StringCallback parent_callback_; + + rclcpp::Subscription::SharedPtr string_subscriber_; + rclcpp::Publisher::SharedPtr string_publisher_; + + std::string content_; +}; +} // namespace rdf_loader diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index ab64407cb8..d9823d382c 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/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& node, const std::string& robot_description) - : robot_description_(robot_description) +RDFLoader::RDFLoader(const std::shared_ptr& 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(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 = std::make_unique(); + 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(); + 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& 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 diff --git a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp new file mode 100644 index 0000000000..a2c1cf27c9 --- /dev/null +++ b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp @@ -0,0 +1,157 @@ +/********************************************************************* + * 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!! */ + +#include + +namespace rdf_loader +{ +std::string SynchronizedStringParameter::loadInitialValue(const std::shared_ptr& node, + const std::string& name, StringCallback parent_callback, + bool default_continuous_value, double default_timeout) +{ + node_ = node; + name_ = name; + parent_callback_ = parent_callback; + + if (getMainParameter()) + { + if (shouldPublish()) + { + // Transient local is similar to latching in ROS 1. + string_publisher_ = node_->create_publisher(name_, rclcpp::QoS(1).transient_local()); + + std_msgs::msg::String msg; + msg.data = content_; + string_publisher_->publish(msg); + } + return content_; + } + + // Load topic parameters + std::string keep_open_param = name_ + "_continuous"; + if (!node_->has_parameter(keep_open_param)) + { + node_->declare_parameter(keep_open_param, default_continuous_value); + } + bool keep_open; + node_->get_parameter_or(keep_open_param, keep_open, default_continuous_value); + + std::string timeout_param = name_ + "_timeout"; + if (!node_->has_parameter(timeout_param)) + { + node_->declare_parameter(timeout_param, default_timeout); + } + double d_timeout; + node_->get_parameter_or(timeout_param, d_timeout, default_timeout); // ten second default + rclcpp::Duration timeout = rclcpp::Duration::from_seconds(d_timeout); + + if (!waitForMessage(timeout)) + { + RCLCPP_ERROR_ONCE(node_->get_logger(), + "Could not find parameter %s and did not receive %s via std_msgs::msg::String subscription " + "within %f seconds.", + name_.c_str(), name_.c_str(), d_timeout); + } + if (!keep_open) + { + string_subscriber_.reset(); + } + return content_; +} + +bool SynchronizedStringParameter::getMainParameter() +{ + // Check if the parameter is declared, declare it if it's not declared yet + if (!node_->has_parameter(name_)) + { + node_->declare_parameter(name_, std::string()); + } + + node_->get_parameter_or(name_, content_, std::string()); + + return !content_.empty(); +} + +bool SynchronizedStringParameter::shouldPublish() +{ + std::string publish_param = "publish_" + name_; + bool publish_string; + if (!node_->has_parameter(publish_param)) + { + node_->declare_parameter(publish_param, false); + } + node_->get_parameter_or(publish_param, publish_string, false); + return publish_string; +} + +bool SynchronizedStringParameter::waitForMessage(const rclcpp::Duration timeout) +{ + 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(), + [this](const std_msgs::msg::String::SharedPtr msg) { return stringCallback(msg); }); + + rclcpp::WaitSet wait_set; + wait_set.add_subscription(string_subscriber_); + + auto ret = wait_set.wait(timeout.to_chrono>()); + if (ret.kind() == rclcpp::WaitResultKind::Ready) + { + std_msgs::msg::String msg; + rclcpp::MessageInfo info; + if (string_subscriber_->take(msg, info)) + { + content_ = msg.data; + return true; + } + } + return false; +} + +void SynchronizedStringParameter::stringCallback(const std_msgs::msg::String::SharedPtr msg) +{ + if (msg->data == content_) + { + return; + } + if (parent_callback_) + { + parent_callback_(msg->data); + } + content_ = msg->data; +} +} // namespace rdf_loader diff --git a/moveit_ros/planning/rdf_loader/test/boring_string_publisher.cpp b/moveit_ros/planning/rdf_loader/test/boring_string_publisher.cpp new file mode 100644 index 0000000000..9d1bfdc8e6 --- /dev/null +++ b/moveit_ros/planning/rdf_loader/test/boring_string_publisher.cpp @@ -0,0 +1,86 @@ +/********************************************************************* + * 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!! */ + +#include +#include +#include +#include +#include + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("boring_string_publisher"); + + // Get Two String Parameters + std::string topic, content_path; + node->declare_parameter("topic", ""); + node->get_parameter("topic", topic); + node->declare_parameter("content_path", ""); + node->get_parameter("content_path", content_path); + + if (content_path.empty()) + { + RCLCPP_FATAL(node->get_logger(), "content_path parameter was not specified or is empty"); + return -1; + } + + // Check if content exists + struct stat statistics; + if (stat(content_path.c_str(), &statistics) != 0) + { + RCLCPP_FATAL(node->get_logger(), "%s does not exist!", content_path.c_str()); + return -1; + } + + // Set Up Publisher + rclcpp::Publisher::SharedPtr string_publisher = + node->create_publisher(topic, rclcpp::QoS(1).transient_local()); + + // Read in Content + std::ifstream content_stream(content_path.c_str()); + std::stringstream buffer; + buffer << content_stream.rdbuf(); + + // Publish Content + std_msgs::msg::String msg; + msg.data = buffer.str(); + string_publisher->publish(msg); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/moveit_ros/planning/rdf_loader/test/data/gonzo.srdf b/moveit_ros/planning/rdf_loader/test/data/gonzo.srdf new file mode 100644 index 0000000000..1acd3e4667 --- /dev/null +++ b/moveit_ros/planning/rdf_loader/test/data/gonzo.srdf @@ -0,0 +1,6 @@ + + + + + + diff --git a/moveit_ros/planning/rdf_loader/test/data/gonzo.urdf b/moveit_ros/planning/rdf_loader/test/data/gonzo.urdf new file mode 100644 index 0000000000..dca3e91bf1 --- /dev/null +++ b/moveit_ros/planning/rdf_loader/test/data/gonzo.urdf @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_ros/planning/rdf_loader/test/data/kermit.srdf b/moveit_ros/planning/rdf_loader/test/data/kermit.srdf new file mode 100644 index 0000000000..4d98fe3b62 --- /dev/null +++ b/moveit_ros/planning/rdf_loader/test/data/kermit.srdf @@ -0,0 +1,6 @@ + + + + + + diff --git a/moveit_ros/planning/rdf_loader/test/data/kermit.urdf b/moveit_ros/planning/rdf_loader/test/data/kermit.urdf new file mode 100644 index 0000000000..dc5d36ceb3 --- /dev/null +++ b/moveit_ros/planning/rdf_loader/test/data/kermit.urdf @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_ros/planning/rdf_loader/test/data/robin.srdf.xacro b/moveit_ros/planning/rdf_loader/test/data/robin.srdf.xacro new file mode 100644 index 0000000000..6ebcdbc339 --- /dev/null +++ b/moveit_ros/planning/rdf_loader/test/data/robin.srdf.xacro @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/moveit_ros/planning/rdf_loader/test/launch/test_rdf_integration.test.py b/moveit_ros/planning/rdf_loader/test/launch/test_rdf_integration.test.py new file mode 100644 index 0000000000..cd89643ede --- /dev/null +++ b/moveit_ros/planning/rdf_loader/test/launch/test_rdf_integration.test.py @@ -0,0 +1,78 @@ +from ament_index_python.packages import get_package_share_path +from launch import LaunchDescription + +from launch_ros.actions import Node + +import launch_testing + +import unittest + + +def generate_test_description(): + rdf_path = get_package_share_path("moveit_ros_planning") / "rdf_loader" + test_data_path = rdf_path / "test" / "data" + + kermit_urdf = open(test_data_path / "kermit.urdf").read() + kermit_srdf = open(test_data_path / "kermit.srdf").read() + + boring_urdf_node = Node( + package="moveit_ros_planning", + executable="boring_string_publisher", + name="urdf_pub0", + parameters=[ + { + "topic": "topic_description", + "content_path": str(test_data_path / "gonzo.urdf"), + } + ], + ) + boring_srdf_node = Node( + package="moveit_ros_planning", + executable="boring_string_publisher", + name="srdf_pub0", + parameters=[ + { + "topic": "topic_description_semantic", + "content_path": str(test_data_path / "gonzo.srdf"), + } + ], + ) + + integration_node = Node( + package="moveit_ros_planning", + executable="test_rdf_integration", + parameters=[ + { + "robot_description": kermit_urdf, + "robot_description_semantic": kermit_srdf, + # The following timeouts are not strictly needed, but make the test run quicker + "DNE_timeout": 1.0, + "DNE_semantic_timeout": 1.0, + } + ], + ) + + return ( + LaunchDescription( + [ + integration_node, + boring_urdf_node, + boring_srdf_node, + launch_testing.actions.ReadyToTest(), + ] + ), + {"integration_node": integration_node}, + ) + + +class TestGTestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_gtest_run_complete(self, proc_info, integration_node): + proc_info.assertWaitForShutdown(integration_node, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_gtest_pass(self, proc_info, integration_node): + launch_testing.asserts.assertExitCodes(proc_info, process=integration_node) diff --git a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp new file mode 100644 index 0000000000..9d813402b5 --- /dev/null +++ b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp @@ -0,0 +1,110 @@ +/********************************************************************* + * 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!! */ + +#include +#include +#include + +TEST(RDFIntegration, default_arguments) +{ + rclcpp::Node::SharedPtr node = std::make_shared("default_arguments"); + rdf_loader::RDFLoader loader(node); + ASSERT_NE(nullptr, loader.getURDF()); + EXPECT_EQ("kermit", loader.getURDF()->name_); + ASSERT_NE(nullptr, loader.getSRDF()); + EXPECT_EQ("kermit", loader.getSRDF()->getName()); +} + +TEST(RDFIntegration, non_existent) +{ + rclcpp::Node::SharedPtr node = std::make_shared("non_existent"); + rdf_loader::RDFLoader loader(node, "does_not_exist"); + ASSERT_EQ(nullptr, loader.getURDF()); + ASSERT_EQ(nullptr, loader.getSRDF()); +} + +TEST(RDFIntegration, topic_based) +{ + rclcpp::Node::SharedPtr node = std::make_shared("topic_based"); + rdf_loader::RDFLoader loader(node, "topic_description"); + ASSERT_NE(nullptr, loader.getURDF()); + EXPECT_EQ("gonzo", loader.getURDF()->name_); + ASSERT_NE(nullptr, loader.getSRDF()); + 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. + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread spinning_thread([&executor] { executor.spin(); }); + + // 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()); + executor.cancel(); + spinning_thread.join(); +} + +TEST(RDFIntegration, xacro_test) +{ + std::string output; + std::vector xacro_args; + ASSERT_TRUE(rdf_loader::RDFLoader::loadPkgFileToString(output, "moveit_ros_planning", + "rdf_loader/test/data/robin.srdf.xacro", xacro_args)); + EXPECT_GT(output.size(), 0u); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h index 68734864b9..53fa26ce2c 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h @@ -101,6 +101,7 @@ private Q_SLOTS: void changedAllLinks(); protected: + void initializeLoader(); void loadRobotModel(); /** diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 5aab9cf031..88630e6be9 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -376,7 +376,7 @@ void RobotStateDisplay::unsetLinkColor(rviz_default_plugins::robot::Robot* robot // ****************************************************************************************** // Load // ****************************************************************************************** -void RobotStateDisplay::loadRobotModel() +void RobotStateDisplay::initializeLoader() { if (robot_description_property_->getStdString().empty()) { @@ -384,8 +384,13 @@ void RobotStateDisplay::loadRobotModel() return; } - rdf_loader_.reset(new rdf_loader::RDFLoader(node_, robot_description_property_->getStdString())); + rdf_loader_.reset(new rdf_loader::RDFLoader(node_, robot_description_property_->getStdString(), true)); + loadRobotModel(); + rdf_loader_->setNewModelCallback([this]() { return loadRobotModel(); }); +} +void RobotStateDisplay::loadRobotModel() +{ if (rdf_loader_->getURDF()) { try @@ -429,7 +434,7 @@ void RobotStateDisplay::onEnable() { Display::onEnable(); if (!rdf_loader_) - loadRobotModel(); + initializeLoader(); changedRobotStateTopic(); calculateOffsetPosition(); }