Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[foxy] Latched Strings for URDF and SRDF #1414

Merged
merged 1 commit into from
Jul 28, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 28 additions & 1 deletion moveit_ros/planning/rdf_loader/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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()
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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*/
Expand All @@ -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);

Expand All @@ -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_;
};
Expand Down
Original file line number Diff line number Diff line change
@@ -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
114 changes: 59 additions & 55 deletions moveit_ros/planning/rdf_loader/src/rdf_loader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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");
Expand All @@ -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;
Expand Down Expand Up @@ -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
Loading