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.

Sign up for GitHub

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

Jump to bottom

Port moveit_core/utils to ROS 2 #68

Merged
merged 5 commits into from May 21, 2019
Merged

Port moveit_core/utils to ROS 2 #68

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
884d498
Port moveit_core/utils to ROS 2
vmayoral Mar 31, 2019
97b08c1
moveit_core/utils adapt to logging https://github.com/ros-planning/mo…
vmayoral Mar 31, 2019
8625f6e
moveit_core utils - xmlrpc_casts.cpp will not be used anymore since X…
ahcorde May 16, 2019
abcc796
moveit_core utils - fixing CmakeLists.txt
ahcorde May 16, 2019
2af5c79
moveit_core utils - restoring addLinkCollision headers
ahcorde May 21, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
40 changes: 24 additions & 16 deletions moveit_core/utils/CMakeLists.txt
View file
Open in desktop
@@ -1,35 +1,43 @@
set(MOVEIT_LIB_NAME moveit_utils)

add_library(${MOVEIT_LIB_NAME}
add_library(${MOVEIT_LIB_NAME} SHARED
src/lexical_casts.cpp
src/xmlrpc_casts.cpp
)
target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})

include_directories(include)
ament_target_dependencies(${MOVEIT_LIB_NAME} visualization_msgs)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

install(TARGETS ${MOVEIT_LIB_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
LIBRARY DESTINATION "lib"
ARCHIVE DESTINATION "lib")

if(CATKIN_ENABLE_TESTING)
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(moveit_resources REQUIRED)
set(MOVEIT_TEST_LIB_NAME moveit_test_utils)
add_library(${MOVEIT_TEST_LIB_NAME}
src/robot_model_test_utils.cpp)

add_dependencies(${MOVEIT_TEST_LIB_NAME} ${catkin_EXPORTED_TARGETS})

find_package(moveit_resources REQUIRED)
include_directories(${moveit_resources_INCLUDE_DIRS})
include_directories(${moveit_resources_INCLUDE_DIRS})

target_link_libraries(${MOVEIT_TEST_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
target_link_libraries(${MOVEIT_TEST_LIB_NAME}
${Boost_LIBRARIES}
${urdf_LIBRARIES}
${srdfdom_LIBRARIES}
${urdfdom_LIBRARIES}
${urdfdom_headers_LIBRARIES}
)

set_target_properties(${MOVEIT_TEST_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

install(TARGETS ${MOVEIT_TEST_LIB_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
endif()
ament_export_libraries(${MOVEIT_TEST_LIB_NAME} ${MOVEIT_LIB_NAME})
ament_export_include_directories(include)

install(TARGETS
${MOVEIT_TEST_LIB_NAME}
LIBRARY DESTINATION "lib"
ARCHIVE DESTINATION "lib")
endif()

install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
install(DIRECTORY include/ DESTINATION "include")
16 changes: 8 additions & 8 deletions moveit_core/utils/include/moveit/utils/robot_model_test_utils.h
View file
Open in desktop
Expand Up @@ -47,7 +47,7 @@
#include <boost/filesystem/path.hpp>
#include <moveit_resources/config.h>
#include <moveit/robot_model/robot_model.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/msg/pose.hpp>

namespace moveit
{
Expand Down Expand Up @@ -110,36 +110,36 @@ class RobotModelBuilder
* origins will default to the identity transform
*/
void addChain(const std::string& section, const std::string& type,
const std::vector<geometry_msgs::Pose>& joint_origins = {});
const std::vector<geometry_msgs::msg::Pose>& joint_origins = {});

/** \brief Adds a collision mesh to a specific link.
* \param[in] link_name The name of the link to which the mesh will be added. Must already be in the builder
* \param[in] filename The path to the mesh file, e.g.
* "package://moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl"
* \param[in] origin The origin pose of this collision mesh relative to the link origin
*/
void addCollisionMesh(const std::string& link_name, const std::string& filename, geometry_msgs::Pose origin);
void addCollisionMesh(const std::string& link_name, const std::string& filename, geometry_msgs::msg::Pose origin);
/** \brief Adds a collision box to a specific link.
* \param[in] link_name The name of the link to which the box will be added. Must already be in the builder.
* \param[in] size The dimensions of the box
* \param[in] origin The origin pose of this collision box relative to the link origin
*/
void addCollisionBox(const std::string& link_name, const std::vector<double>& dims, geometry_msgs::Pose origin);
void addCollisionBox(const std::string& link_name, const std::vector<double>& dims, geometry_msgs::msg::Pose origin);

/** \brief Adds a visual box to a specific link.
* \param[in] link_name The name of the link to which the box will be added. Must already be in the builder.
* \param[in] size The dimensions of the box
* \param[in] origin The origin pose of this visual box relative to the link origin
*/
void addVisualBox(const std::string& link_name, const std::vector<double>& size, geometry_msgs::Pose origin);
void addVisualBox(const std::string& link_name, const std::vector<double>& size, geometry_msgs::msg::Pose origin);

/**
* Adds an inertial component to a link.
* \param[in] link_name The name of the link for this inertial information
* \param[in] mass The mass of the link
* \param[in] origin The origin center pose of the center of mass of this link
*/
void addInertial(const std::string& link_name, double mass, geometry_msgs::Pose origin, double ixx, double ixy,
void addInertial(const std::string& link_name, double mass, geometry_msgs::msg::Pose origin, double ixx, double ixy,
double ixz, double iyy, double iyz, double izz);

/** \} */
Expand Down Expand Up @@ -182,10 +182,10 @@ class RobotModelBuilder

private:
/** \brief Adds different collision geometries to a link. */
void addLinkCollision(const std::string& link_name, const urdf::CollisionSharedPtr& coll, geometry_msgs::Pose origin);
void addLinkCollision(const std::string& link_name, urdf::CollisionSharedPtr coll, geometry_msgs::msg::Pose origin);
mlautman marked this conversation as resolved.
Show resolved Hide resolved

/** \brief Adds different visual geometries to a link. */
void addLinkVisual(const std::string& link_name, const urdf::VisualSharedPtr& vis, geometry_msgs::Pose origin);
void addLinkVisual(const std::string& link_name, urdf::VisualSharedPtr vis, geometry_msgs::msg::Pose origin);

/// The URDF model, holds all of the URDF components of the robot added so far.
urdf::ModelInterfaceSharedPtr urdf_model_;
Expand Down
64 changes: 0 additions & 64 deletions moveit_core/utils/include/moveit/utils/xmlrpc_casts.h
View file
Open in desktop

This file was deleted.

1 change: 0 additions & 1 deletion moveit_core/utils/src/lexical_casts.cpp
View file
Open in desktop
Expand Up @@ -35,7 +35,6 @@
/* Author: Simon Schmeisser */

#include "moveit/utils/lexical_casts.h"

#include <locale>
#include <sstream>

Expand Down
50 changes: 25 additions & 25 deletions moveit_core/utils/src/robot_model_test_utils.cpp
View file
Open in desktop
Expand Up @@ -34,17 +34,17 @@

/* Author: Bryce Willey */

#include <ros/ros.h>
#include "rclcpp/rclcpp.hpp"
#include <boost/algorithm/string_regex.hpp>
#include <boost/math/constants/constants.hpp>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/msg/pose.hpp>
#include "moveit/utils/robot_model_test_utils.h"

namespace moveit
{
namespace core
{
const std::string LOGNAME = "robot_model_builder";
rclcpp::Logger LOGGER_UTILS = rclcpp::get_logger("moveit").get_child("robot_model_builder");

moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name)
{
Expand All @@ -69,7 +69,7 @@ urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name)
urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path);
if (urdf_model == nullptr)
{
ROS_ERROR_NAMED(LOGNAME, "Cannot find URDF for %s. Make sure moveit_resources/your robot description is installed",
RCLCPP_ERROR(LOGGER_UTILS, "Cannot find URDF for %s. Make sure moveit_resources/your robot description is installed",
robot_name.c_str());
}
return urdf_model;
Expand Down Expand Up @@ -107,27 +107,27 @@ RobotModelBuilder::RobotModelBuilder(const std::string& name, const std::string&
}

void RobotModelBuilder::addChain(const std::string& section, const std::string& type,
const std::vector<geometry_msgs::Pose>& joint_origins)
const std::vector<geometry_msgs::msg::Pose>& joint_origins)
{
std::vector<std::string> link_names;
boost::split_regex(link_names, section, boost::regex("->"));
if (link_names.empty())
{
ROS_ERROR_NAMED(LOGNAME, "No links specified (empty section?)");
RCLCPP_ERROR(LOGGER_UTILS, "No links specified (empty section?)");
is_valid_ = false;
return;
}
// First link should already be added.
if (not urdf_model_->getLink(link_names[0]))
{
ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_names[0].c_str());
RCLCPP_ERROR(LOGGER_UTILS, "Link %s not present in builder yet!", link_names[0].c_str());
is_valid_ = false;
return;
}

if (not joint_origins.empty() && link_names.size() - 1 != joint_origins.size())
{
ROS_ERROR_NAMED(LOGNAME, "There should be one more link (%zu) than there are joint origins (%zu)",
RCLCPP_ERROR(LOGGER_UTILS, "There should be one more link (%zu) than there are joint origins (%zu)",
link_names.size(), joint_origins.size());
is_valid_ = false;
return;
Expand All @@ -139,7 +139,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string&
// These links shouldn't be present already.
if (urdf_model_->getLink(link_names[i]))
{
ROS_ERROR_NAMED(LOGNAME, "Link %s is already specified", link_names[i].c_str());
RCLCPP_ERROR(LOGGER_UTILS, "Link %s is already specified", link_names[i].c_str());
is_valid_ = false;
return;
}
Expand All @@ -152,7 +152,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string&
joint->parent_to_joint_origin_transform.clear();
if (not joint_origins.empty())
{
geometry_msgs::Pose o = joint_origins[i - 1];
geometry_msgs::msg::Pose o = joint_origins[i - 1];
joint->parent_to_joint_origin_transform.position = urdf::Vector3(o.position.x, o.position.y, o.position.z);
joint->parent_to_joint_origin_transform.rotation =
urdf::Rotation(o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w);
Expand All @@ -174,7 +174,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string&
joint->type = urdf::Joint::FIXED;
else
{
ROS_ERROR_NAMED(LOGNAME, "No such joint type as %s", type.c_str());
RCLCPP_ERROR(LOGGER_UTILS, "No such joint type as %s", type.c_str());
is_valid_ = false;
return;
}
Expand All @@ -192,12 +192,12 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string&
}
}

void RobotModelBuilder::addInertial(const std::string& link_name, double mass, geometry_msgs::Pose origin, double ixx,
void RobotModelBuilder::addInertial(const std::string& link_name, double mass, geometry_msgs::msg::Pose origin, double ixx,
double ixy, double ixz, double iyy, double iyz, double izz)
{
if (not urdf_model_->getLink(link_name))
{
ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
RCLCPP_ERROR(LOGGER_UTILS, "Link %s not present in builder yet!", link_name.c_str());
is_valid_ = false;
return;
}
Expand All @@ -220,7 +220,7 @@ void RobotModelBuilder::addInertial(const std::string& link_name, double mass, g
}

void RobotModelBuilder::addVisualBox(const std::string& link_name, const std::vector<double>& size,
geometry_msgs::Pose origin)
geometry_msgs::msg::Pose origin)
{
urdf::VisualSharedPtr vis(new urdf::Visual);
urdf::BoxSharedPtr geometry(new urdf::Box);
Expand All @@ -230,11 +230,11 @@ void RobotModelBuilder::addVisualBox(const std::string& link_name, const std::ve
}

void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std::vector<double>& dims,
geometry_msgs::Pose origin)
geometry_msgs::msg::Pose origin)
{
if (dims.size() != 3)
{
ROS_ERROR("There can only be 3 dimensions of a box (given %zu!)", dims.size());
RCLCPP_ERROR(LOGGER_UTILS,"There can only be 3 dimensions of a box (given %zu!)");
is_valid_ = false;
return;
}
Expand All @@ -246,7 +246,7 @@ void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std:
}

void RobotModelBuilder::addCollisionMesh(const std::string& link_name, const std::string& filename,
geometry_msgs::Pose origin)
geometry_msgs::msg::Pose origin)
{
urdf::CollisionSharedPtr coll(new urdf::Collision);
urdf::MeshSharedPtr geometry(new urdf::Mesh);
Expand All @@ -255,12 +255,12 @@ void RobotModelBuilder::addCollisionMesh(const std::string& link_name, const std
addLinkCollision(link_name, coll, origin);
}

void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urdf::CollisionSharedPtr& collision,
geometry_msgs::Pose origin)
void RobotModelBuilder::addLinkCollision(const std::string& link_name, urdf::CollisionSharedPtr collision,
geometry_msgs::msg::Pose origin)
{
if (not urdf_model_->getLink(link_name))
{
ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
RCLCPP_ERROR(LOGGER_UTILS, "Link %s not present in builder yet!", link_name.c_str());
is_valid_ = false;
return;
}
Expand All @@ -273,12 +273,12 @@ void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urd
link->collision_array.push_back(collision);
}

void RobotModelBuilder::addLinkVisual(const std::string& link_name, const urdf::VisualSharedPtr& vis,
geometry_msgs::Pose origin)
void RobotModelBuilder::addLinkVisual(const std::string& link_name, urdf::VisualSharedPtr vis,
geometry_msgs::msg::Pose origin)
{
if (not urdf_model_->getLink(link_name))
{
ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
RCLCPP_ERROR(LOGGER_UTILS, "Link %s not present in builder yet!", link_name.c_str());
is_valid_ = false;
return;
}
Expand Down Expand Up @@ -357,7 +357,7 @@ moveit::core::RobotModelPtr RobotModelBuilder::build()
}
catch (urdf::ParseError& e)
{
ROS_ERROR_NAMED(LOGNAME, "Failed to build tree: %s", e.what());
RCLCPP_ERROR(LOGGER_UTILS, "Failed to build tree: %s", e.what());
return robot_model;
}

Expand All @@ -368,7 +368,7 @@ moveit::core::RobotModelPtr RobotModelBuilder::build()
}
catch (urdf::ParseError& e)
{
ROS_ERROR_NAMED(LOGNAME, "Failed to find root link: %s", e.what());
RCLCPP_ERROR(LOGGER_UTILS, "Failed to find root link: %s", e.what());
return robot_model;
}
srdf_writer_->updateSRDFModel(*urdf_model_);
Expand Down