From 7599e6b328d3b2795bee62d252568e67e5ce3a22 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 16 May 2019 12:12:01 +0200 Subject: [PATCH 01/13] Port robot_state to ROS2 Co-authored-by: vmayoral Co-authored-by: anasarrak --- moveit_core/robot_state/CMakeLists.txt | 25 ++- .../moveit/robot_state/attached_body.h | 8 +- .../include/moveit/robot_state/conversions.h | 6 +- .../include/moveit/robot_state/log.h | 53 +++++++ .../include/moveit/robot_state/robot_state.h | 97 ++---------- moveit_core/robot_state/src/attached_body.cpp | 2 +- moveit_core/robot_state/src/conversions.cpp | 75 ++++----- moveit_core/robot_state/src/robot_state.cpp | 111 ++++++------- moveit_core/robot_state/test/test_aabb.cpp | 146 +++++++++--------- 9 files changed, 256 insertions(+), 267 deletions(-) create mode 100644 moveit_core/robot_state/include/moveit/robot_state/log.h diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 4f53cc428f..59310d3e8d 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -1,25 +1,20 @@ set(MOVEIT_LIB_NAME moveit_robot_state) -if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - # clang is picky about typeid(*solver) - add_compile_options(-Wno-potentially-evaluated-expression) -endif() - add_library(${MOVEIT_LIB_NAME} src/attached_body.cpp src/conversions.cpp src/robot_state.cpp ) -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) -target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model moveit_kinematics_base moveit_transforms ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model moveit_kinematics_base moveit_transforms ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) +install(DIRECTORY include/ DESTINATION include) # Unit tests if(CATKIN_ENABLE_TESTING) @@ -27,15 +22,15 @@ if(CATKIN_ENABLE_TESTING) include_directories(${moveit_resources_INCLUDE_DIRS}) catkin_add_gtest(test_robot_state test/robot_state_test.cpp) - target_link_libraries(test_robot_state moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_robot_state moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) # As an executable, this benchmark is not run as a test by default add_executable(robot_state_benchmark test/robot_state_benchmark.cpp) - target_link_libraries(robot_state_benchmark moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME} ${GTEST_LIBRARIES}) + target_link_libraries(robot_state_benchmark moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME} ${GTEST_LIBRARIES}) catkin_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) - target_link_libraries(test_robot_state_complex moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_robot_state_complex moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) catkin_add_gtest(test_aabb test/test_aabb.cpp) - target_link_libraries(test_aabb moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_aabb moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) endif() diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index 1ce197892d..4c2fc0c737 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include namespace moveit @@ -62,7 +62,7 @@ class AttachedBody object is specified by \e touch_links. */ AttachedBody(const LinkModel* link, const std::string& id, const std::vector& shapes, const EigenSTL::vector_Isometry3d& attach_trans, const std::set& touch_links, - const trajectory_msgs::JointTrajectory& attach_posture); + const trajectory_msgs::msg::JointTrajectory& attach_posture); ~AttachedBody(); @@ -99,7 +99,7 @@ class AttachedBody /** \brief Return the posture that is necessary for the object to be released, (if any). This is useful for example when storing the configuration of a gripper holding an object */ - const trajectory_msgs::JointTrajectory& getDetachPosture() const + const trajectory_msgs::msg::JointTrajectory& getDetachPosture() const { return detach_posture_; } @@ -147,7 +147,7 @@ class AttachedBody /** \brief Posture of links for releasing the object (if any). This is useful for example when storing the configuration of a gripper holding an object */ - trajectory_msgs::JointTrajectory detach_posture_; + trajectory_msgs::msg::JointTrajectory detach_posture_; /** \brief The global transforms for these attached bodies (computed by forward kinematics) */ EigenSTL::vector_Isometry3d global_collision_body_transforms_; diff --git a/moveit_core/robot_state/include/moveit/robot_state/conversions.h b/moveit_core/robot_state/include/moveit/robot_state/conversions.h index 99c9da47ea..97147a37e8 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h @@ -52,7 +52,7 @@ namespace core * @param state The resultant MoveIt! robot state * @return True if successful, false if failed for any reason */ -bool jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state); +bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, RobotState& state); /** * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state @@ -97,7 +97,7 @@ void attachedBodiesToAttachedCollisionObjectMsgs( * @param state The input MoveIt! robot state object * @param robot_state The resultant JointState message */ -void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& joint_state); +void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::msg::JointState& joint_state); /** * @brief Convert a joint trajectory point to a MoveIt! robot state @@ -106,7 +106,7 @@ void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& * @param state The resultant MoveIt! robot state * @return True if successful, false if failed for any reason */ -bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory& trajectory, std::size_t point_id, +bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& trajectory, std::size_t point_id, RobotState& state); /** diff --git a/moveit_core/robot_state/include/moveit/robot_state/log.h b/moveit_core/robot_state/include/moveit/robot_state/log.h new file mode 100644 index 0000000000..99eabfd87d --- /dev/null +++ b/moveit_core/robot_state/include/moveit/robot_state/log.h @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Acutronic Robotics AG + * 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 Willow Garage 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: Víctor Mayoral Vilches */ + +#ifndef MOVEIT_CORE_ROBOT_STATE_LOG_ +#define MOVEIT_CORE_ROBOT_STATE_LOG_ + +#include "rclcpp/rclcpp.hpp" + +namespace moveit +{ +namespace core +{ + +// Logger +rclcpp::Logger logger_robot_state = rclcpp::get_logger("robot_state"); + +}; +}; + +#endif diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 29aaeef0f5..f94b2a3ff1 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -41,10 +41,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -955,13 +955,6 @@ as the new values that correspond to the group */ bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int attempts, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) - { - return setFromIK(group, pose, timeout, constraint, options); - } /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. @@ -970,17 +963,9 @@ as the new values that correspond to the group */ @param tip The name of the link the pose is specified for @param timeout The timeout passed to the kinematics solver on each attempt @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip, + bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, const std::string& tip, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip, - unsigned int attempts, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) - { - return setFromIK(group, pose, tip, timeout, constraint, options); - } /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. @@ -991,13 +976,6 @@ as the new values that correspond to the group */ bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int attempts, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) - { - return setFromIK(group, pose, timeout, constraint, options); - } /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. @@ -1008,13 +986,6 @@ as the new values that correspond to the group */ bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, unsigned int attempts, - double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) - { - return setFromIK(group, pose, tip, timeout, constraint, options); - } /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. @@ -1028,14 +999,6 @@ as the new values that correspond to the group */ const std::vector& consistency_limits, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, - const std::vector& consistency_limits, unsigned int attempts, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) - { - return setFromIK(group, pose, tip, consistency_limits, timeout, constraint, options); - } /** \brief Warning: This function inefficiently copies all transforms around. If the group consists of a set of sub-groups that are each a chain and a solver @@ -1050,14 +1013,6 @@ as the new values that correspond to the group */ const std::vector& tips, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, unsigned int attempts, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) - { - return setFromIK(group, poses, tips, timeout, constraint, options); - } /** \brief Warning: This function inefficiently copies all transforms around. If the group consists of a set of sub-groups that are each a chain and a solver @@ -1073,15 +1028,6 @@ as the new values that correspond to the group */ const std::vector& tips, const std::vector >& consistency_limits, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, const std::vector >& consistency_limits, - unsigned int attempts, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) - { - return setFromIK(group, poses, tips, consistency_limits, timeout, constraint, options); - } /** \brief setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solver for @@ -1096,15 +1042,6 @@ as the new values that correspond to the group */ const std::vector >& consistency_limits, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, const std::vector >& consistency_limits, - unsigned int attempts, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) - { - return setFromIKSubgroups(group, poses, tips, consistency_limits, timeout, constraint, options); - } /** \brief Set the joint values from a Cartesian velocity applied during a time dt * @param group the group of joints this function operates on @@ -1123,7 +1060,7 @@ as the new values that correspond to the group */ * @param dt a time interval (seconds) * @param st a secondary task computation function */ - bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt, + bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::msg::Twist& twist, const std::string& tip, double dt, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn()); /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular group. @@ -1340,7 +1277,7 @@ as the new values that correspond to the group */ * @{ */ - void setVariableValues(const sensor_msgs::JointState& msg) + void setVariableValues(const sensor_msgs::msg::JointState& msg) { if (!msg.position.empty()) setVariablePositions(msg.name, msg.position); @@ -1661,7 +1598,7 @@ as the new values that correspond to the group */ void attachBody(const std::string& id, const std::vector& shapes, const EigenSTL::vector_Isometry3d& attach_trans, const std::set& touch_links, const std::string& link_name, - const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory()); + const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory()); /** @brief Add an attached body to a link * @param id The string id associated with the attached body @@ -1680,7 +1617,7 @@ as the new values that correspond to the group */ void attachBody(const std::string& id, const std::vector& shapes, const EigenSTL::vector_Isometry3d& attach_trans, const std::vector& touch_links, const std::string& link_name, - const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory()) + const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory()) { std::set touch_links_set(touch_links.begin(), touch_links.end()); attachBody(id, shapes, attach_trans, touch_links_set, link_name, detach_posture); @@ -1751,10 +1688,10 @@ as the new values that correspond to the group */ * @param link_names The list of link names for which the markers should be created. * @param color The color for the marker * @param ns The namespace for the markers - * @param dur The ros::Duration for which the markers should stay visible + * @param dur The rclcpp::Duration for which the markers should stay visible */ - void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector& link_names, - const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur, + void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, + const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::Duration& dur, bool include_attached = false) const; /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first. @@ -1762,10 +1699,10 @@ as the new values that correspond to the group */ * @param link_names The list of link names for which the markers should be created. * @param color The color for the marker * @param ns The namespace for the markers - * @param dur The ros::Duration for which the markers should stay visible + * @param dur The rclcpp::Duration for which the markers should stay visible */ - void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector& link_names, - const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur, + void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, + const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::Duration& dur, bool include_attached = false) { updateCollisionBodyTransforms(); @@ -1776,14 +1713,14 @@ as the new values that correspond to the group */ * @param arr The returned marker array * @param link_names The list of link names for which the markers should be created. */ - void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector& link_names, + void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, bool include_attached = false) const; /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first. * @param arr The returned marker array * @param link_names The list of link names for which the markers should be created. */ - void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector& link_names, + void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, bool include_attached = false) { updateCollisionBodyTransforms(); diff --git a/moveit_core/robot_state/src/attached_body.cpp b/moveit_core/robot_state/src/attached_body.cpp index 97722c99f9..0899901e8f 100644 --- a/moveit_core/robot_state/src/attached_body.cpp +++ b/moveit_core/robot_state/src/attached_body.cpp @@ -41,7 +41,7 @@ moveit::core::AttachedBody::AttachedBody(const LinkModel* parent_link_model, con const std::vector& shapes, const EigenSTL::vector_Isometry3d& attach_trans, const std::set& touch_links, - const trajectory_msgs::JointTrajectory& detach_posture) + const trajectory_msgs::msg::JointTrajectory& detach_posture) : parent_link_model_(parent_link_model) , id_(id) , shapes_(shapes) diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index ae741b82c4..a87c75c22b 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -39,12 +39,12 @@ #include #include #include +#include namespace moveit { namespace core { -const std::string LOGNAME = "robot_state"; // ******************************************** // * Internal (hidden) functions @@ -52,11 +52,11 @@ const std::string LOGNAME = "robot_state"; namespace { -static bool _jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state) +static bool _jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, RobotState& state) { if (joint_state.name.size() != joint_state.position.size()) { - ROS_ERROR_NAMED(LOGNAME, "Different number of names and positions in JointState message: %zu, %zu", + RCLCPP_ERROR(logger_robot_state, "Different number of names and positions in JointState message: %zu, %zu", joint_state.name.size(), joint_state.position.size()); return false; } @@ -66,13 +66,13 @@ static bool _jointStateToRobotState(const sensor_msgs::JointState& joint_state, return true; } -static bool _multiDOFJointsToRobotState(const sensor_msgs::MultiDOFJointState& mjs, RobotState& state, +static bool _multiDOFJointsToRobotState(const sensor_msgs::msg::MultiDOFJointState& mjs, RobotState& state, const Transforms* tf) { std::size_t nj = mjs.joint_names.size(); if (nj != mjs.transforms.size()) { - ROS_ERROR_NAMED(LOGNAME, "Different number of names, values or frames in MultiDOFJointState message."); + RCLCPP_ERROR(logger_robot_state, "Different number of names, values or frames in MultiDOFJointState message."); return false; } @@ -94,14 +94,14 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::MultiDOFJointState& m } catch (std::exception& ex) { - ROS_ERROR_NAMED(LOGNAME, "Caught %s", ex.what()); + RCLCPP_ERROR(logger_robot_state, "Caught %s", ex.what()); error = true; } else error = true; if (error) - ROS_WARN_NAMED(LOGNAME, "The transform for multi-dof joints was specified in frame '%s' " + RCLCPP_WARN(logger_robot_state, "The transform for multi-dof joints was specified in frame '%s' " "but it was not possible to transform that to frame '%s'", mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str()); } @@ -111,7 +111,7 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::MultiDOFJointState& m const std::string& joint_name = mjs.joint_names[i]; if (!state.getRobotModel()->hasJointModel(joint_name)) { - ROS_WARN_NAMED(LOGNAME, "No joint matching multi-dof joint '%s'", joint_name.c_str()); + RCLCPP_WARN(logger_robot_state, "No joint matching multi-dof joint '%s'", joint_name.c_str()); error = true; continue; } @@ -126,14 +126,14 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::MultiDOFJointState& m return !error; } -static inline void _robotStateToMultiDOFJointState(const RobotState& state, sensor_msgs::MultiDOFJointState& mjs) +static inline void _robotStateToMultiDOFJointState(const RobotState& state, sensor_msgs::msg::MultiDOFJointState& mjs) { const std::vector& js = state.getRobotModel()->getMultiDOFJointModels(); mjs.joint_names.clear(); mjs.transforms.clear(); for (std::size_t i = 0; i < js.size(); ++i) { - geometry_msgs::TransformStamped p; + geometry_msgs::msg::TransformStamped p; if (state.dirtyJointTransform(js[i])) { Eigen::Isometry3d t; @@ -156,25 +156,25 @@ class ShapeVisitorAddToCollisionObject : public boost::static_visitor { } - void addToObject(const shapes::ShapeMsg& sm, const geometry_msgs::Pose& pose) + void addToObject(const shapes::ShapeMsg& sm, const geometry_msgs::msg::Pose& pose) { pose_ = &pose; boost::apply_visitor(*this, sm); } - void operator()(const shape_msgs::Plane& shape_msg) const + void operator()(const shape_msgs::msg::Plane& shape_msg) const { obj_->planes.push_back(shape_msg); obj_->plane_poses.push_back(*pose_); } - void operator()(const shape_msgs::Mesh& shape_msg) const + void operator()(const shape_msgs::msg::Mesh& shape_msg) const { obj_->meshes.push_back(shape_msg); obj_->mesh_poses.push_back(*pose_); } - void operator()(const shape_msgs::SolidPrimitive& shape_msg) const + void operator()(const shape_msgs::msg::SolidPrimitive& shape_msg) const { obj_->primitives.push_back(shape_msg); obj_->primitive_poses.push_back(*pose_); @@ -182,7 +182,7 @@ class ShapeVisitorAddToCollisionObject : public boost::static_visitor private: moveit_msgs::msg::CollisionObject* obj_; - const geometry_msgs::Pose* pose_; + const geometry_msgs::msg::Pose* pose_; }; static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::msg::AttachedCollisionObject& aco) @@ -211,7 +211,7 @@ static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::m shapes::ShapeMsg sm; if (shapes::constructMsgFromShape(ab_shapes[j].get(), sm)) { - geometry_msgs::Pose p; + geometry_msgs::msg::Pose p; p = tf2::toMsg(ab_tf[j]); sv.addToObject(sm, p); } @@ -226,20 +226,20 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att { if (aco.object.primitives.size() != aco.object.primitive_poses.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of primitive shapes does not match " + RCLCPP_ERROR(logger_robot_state, "Number of primitive shapes does not match " "number of poses in collision object message"); return; } if (aco.object.meshes.size() != aco.object.mesh_poses.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of meshes does not match number of poses in collision object message"); + RCLCPP_ERROR(logger_robot_state, "Number of meshes does not match number of poses in collision object message"); return; } if (aco.object.planes.size() != aco.object.plane_poses.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of planes does not match number of poses in collision object message"); + RCLCPP_ERROR(logger_robot_state, "Number of planes does not match number of poses in collision object message"); return; } @@ -295,7 +295,7 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att else { t0.setIdentity(); - ROS_ERROR_NAMED(LOGNAME, "Cannot properly transform from frame '%s'. " + RCLCPP_ERROR(logger_robot_state, "Cannot properly transform from frame '%s'. " "The pose of the attached body may be incorrect", aco.object.header.frame_id.c_str()); } @@ -304,31 +304,32 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att poses[i] = t * poses[i]; } - if (shapes.empty()) - ROS_ERROR_NAMED(LOGNAME, "There is no geometry to attach to link '%s' as part of attached body '%s'", - aco.link_name.c_str(), aco.object.id.c_str()); + if (shapes.empty()) { + RCLCPP_ERROR(logger_robot_state, "There is no geometry to attach to link '%s' as part of attached body '%s'", + aco.link_name.c_str(), aco.object.id.c_str()); + } else { if (state.clearAttachedBody(aco.object.id)) - ROS_DEBUG_NAMED(LOGNAME, "The robot state already had an object named '%s' attached to link '%s'. " + RCLCPP_DEBUG(logger_robot_state, "The robot state already had an object named '%s' attached to link '%s'. " "The object was replaced.", aco.object.id.c_str(), aco.link_name.c_str()); state.attachBody(aco.object.id, shapes, poses, aco.touch_links, aco.link_name, aco.detach_posture); - ROS_DEBUG_NAMED(LOGNAME, "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str()); + RCLCPP_DEBUG(logger_robot_state, "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str()); } } } else - ROS_ERROR_NAMED(LOGNAME, "The attached body for link '%s' has no geometry", aco.link_name.c_str()); + RCLCPP_ERROR(logger_robot_state, "The attached body for link '%s' has no geometry", aco.link_name.c_str()); } else if (aco.object.operation == moveit_msgs::msg::CollisionObject::REMOVE) { if (!state.clearAttachedBody(aco.object.id)) - ROS_ERROR_NAMED(LOGNAME, "The attached body '%s' can not be removed because it does not exist", + RCLCPP_ERROR(logger_robot_state, "The attached body '%s' can not be removed because it does not exist", aco.link_name.c_str()); } else - ROS_ERROR_NAMED(LOGNAME, "Unknown collision object operation: %d", aco.object.operation); + RCLCPP_ERROR(logger_robot_state, "Unknown collision object operation: %d", aco.object.operation); } static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::msg::RobotState& robot_state, @@ -339,7 +340,7 @@ static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_ if (!rs.is_diff && rs.joint_state.name.empty() && rs.multi_dof_joint_state.joint_names.empty()) { - ROS_ERROR_NAMED(LOGNAME, "Found empty JointState message"); + RCLCPP_ERROR(logger_robot_state, "Found empty JointState message"); return false; } @@ -365,7 +366,7 @@ static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_ // * Exposed functions // ******************************************** -bool jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state) +bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, RobotState& state) { bool result = _jointStateToRobotState(joint_state, state); state.update(); @@ -410,10 +411,10 @@ void attachedBodiesToAttachedCollisionObjectMsgs( _attachedBodyToMsg(*attached_bodies[i], attached_collision_objs[i]); } -void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& joint_state) +void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::msg::JointState& joint_state) { const std::vector& js = state.getRobotModel()->getSingleDOFJointModels(); - joint_state = sensor_msgs::JointState(); + joint_state = sensor_msgs::msg::JointState(); for (std::size_t i = 0; i < js.size(); ++i) { @@ -430,17 +431,17 @@ void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& joint_state.header.frame_id = state.getRobotModel()->getModelFrame(); } -bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory& trajectory, std::size_t point_id, +bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& trajectory, std::size_t point_id, RobotState& state) { if (trajectory.points.empty() || point_id > trajectory.points.size() - 1) { - ROS_ERROR_NAMED(LOGNAME, "Invalid point_id"); + RCLCPP_ERROR(logger_robot_state, "Invalid point_id"); return false; } if (trajectory.joint_names.empty()) { - ROS_ERROR_NAMED(LOGNAME, "No joint names specified"); + RCLCPP_ERROR(logger_robot_state, "No joint names specified"); return false; } @@ -530,8 +531,8 @@ void streamToRobotState(RobotState& state, const std::string& line, const std::s { // Get a variable if (!std::getline(line_stream, cell, separator[0])) - ROS_ERROR_STREAM_NAMED(LOGNAME, "Missing variable " << state.getVariableNames()[i]); - + RCLCPP_ERROR(logger_robot_state, "Missing variable " , state.getVariableNames()[i].c_str()); + printf("show variablename %s\n", state.getVariableNames()[i].c_str()); state.getVariablePositions()[i] = boost::lexical_cast(cell.c_str()); } } diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 79fffbdb45..68dba0a949 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -44,6 +44,8 @@ #include #include #include +#include +#include "rclcpp/rclcpp.hpp" namespace moveit { @@ -55,8 +57,6 @@ namespace core * valid paths from paths with large joint space jumps. */ static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10; -const std::string LOGNAME = "robot_state"; - RobotState::RobotState(const RobotModelConstPtr& robot_model) : robot_model_(robot_model) , has_velocity_(false) @@ -182,7 +182,7 @@ bool RobotState::checkJointTransforms(const JointModel* joint) const { if (dirtyJointTransform(joint)) { - ROS_WARN_NAMED(LOGNAME, "Returning dirty joint transforms for joint '%s'", joint->getName().c_str()); + RCLCPP_WARN(logger_robot_state, "Returning dirty joint transforms for joint '%s'", joint->getName().c_str()); return false; } return true; @@ -192,7 +192,7 @@ bool RobotState::checkLinkTransforms() const { if (dirtyLinkTransforms()) { - ROS_WARN_NAMED(LOGNAME, "Returning dirty link transforms"); + RCLCPP_WARN(logger_robot_state, "Returning dirty link transforms"); return false; } return true; @@ -202,7 +202,7 @@ bool RobotState::checkCollisionTransforms() const { if (dirtyCollisionBodyTransforms()) { - ROS_WARN_NAMED(LOGNAME, "Returning dirty collision body transforms"); + RCLCPP_WARN(logger_robot_state, "Returning dirty collision body transforms"); return false; } return true; @@ -451,7 +451,7 @@ void RobotState::setJointEfforts(const JointModel* joint, const double* effort) { if (has_acceleration_) { - ROS_ERROR_NAMED(LOGNAME, "Unable to set joint efforts because array is being used for accelerations"); + RCLCPP_ERROR(logger_robot_state, "Unable to set joint efforts because array is being used for accelerations"); return; } has_effort_ = true; @@ -880,7 +880,7 @@ const AttachedBody* RobotState::getAttachedBody(const std::string& id) const std::map::const_iterator it = attached_body_map_.find(id); if (it == attached_body_map_.end()) { - ROS_ERROR_NAMED(LOGNAME, "Attached body '%s' not found", id.c_str()); + RCLCPP_ERROR(logger_robot_state, "Attached body '%s' not found", id.c_str()); return nullptr; } else @@ -897,7 +897,7 @@ void RobotState::attachBody(AttachedBody* attached_body) void RobotState::attachBody(const std::string& id, const std::vector& shapes, const EigenSTL::vector_Isometry3d& attach_trans, const std::set& touch_links, - const std::string& link, const trajectory_msgs::JointTrajectory& detach_posture) + const std::string& link, const trajectory_msgs::msg::JointTrajectory& detach_posture) { const LinkModel* l = robot_model_->getLinkModel(link); AttachedBody* ab = new AttachedBody(l, id, shapes, attach_trans, touch_links, detach_posture); @@ -1021,7 +1021,7 @@ const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& id) co std::map::const_iterator jt = attached_body_map_.find(id); if (jt == attached_body_map_.end()) { - ROS_ERROR_NAMED(LOGNAME, "Transform from frame '%s' to frame '%s' is not known " + RCLCPP_ERROR(logger_robot_state, "Transform from frame '%s' to frame '%s' is not known " "('%s' should be a link name or an attached body id).", id.c_str(), robot_model_->getModelFrame().c_str(), id.c_str()); return IDENTITY_TRANSFORM; @@ -1029,12 +1029,12 @@ const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& id) co const EigenSTL::vector_Isometry3d& tf = jt->second->getGlobalCollisionBodyTransforms(); if (tf.empty()) { - ROS_ERROR_NAMED(LOGNAME, "Attached body '%s' has no geometry associated to it. No transform to return.", + RCLCPP_ERROR(logger_robot_state, "Attached body '%s' has no geometry associated to it. No transform to return.", id.c_str()); return IDENTITY_TRANSFORM; } if (tf.size() > 1) - ROS_DEBUG_NAMED(LOGNAME, "There are multiple geometries associated to attached body '%s'. " + RCLCPP_DEBUG(logger_robot_state, "There are multiple geometries associated to attached body '%s'. " "Returning the transform for the first one.", id.c_str()); return tf[0]; @@ -1050,8 +1050,8 @@ bool RobotState::knowsFrameTransform(const std::string& id) const return it != attached_body_map_.end() && !it->second->getGlobalCollisionBodyTransforms().empty(); } -void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector& link_names, - const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur, +void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, + const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::Duration& dur, bool include_attached) const { std::size_t cur_num = arr.markers.size(); @@ -1066,13 +1066,14 @@ void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std } } -void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector& link_names, +void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, bool include_attached) const { - ros::Time tm = ros::Time::now(); + builtin_interfaces::msg::Time tm = rclcpp::Clock().now(); + for (std::size_t i = 0; i < link_names.size(); ++i) { - ROS_DEBUG_NAMED(LOGNAME, "Trying to get marker for link '%s'", link_names[i].c_str()); + RCLCPP_DEBUG(logger_robot_state, "Trying to get marker for link '%s'", link_names[i].c_str()); const LinkModel* lm = robot_model_->getLinkModel(link_names[i]); if (!lm) continue; @@ -1083,7 +1084,7 @@ void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std { for (std::size_t j = 0; j < it->second->getShapes().size(); ++j) { - visualization_msgs::Marker att_mark; + visualization_msgs::msg::Marker att_mark; att_mark.header.frame_id = robot_model_->getModelFrame(); att_mark.header.stamp = tm; if (shapes::constructMarkerFromShape(it->second->getShapes()[j].get(), att_mark)) @@ -1102,7 +1103,7 @@ void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std for (std::size_t j = 0; j < lm->getShapes().size(); ++j) { - visualization_msgs::Marker mark; + visualization_msgs::msg::Marker mark; mark.header.frame_id = robot_model_->getModelFrame(); mark.header.stamp = tm; @@ -1152,13 +1153,13 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link if (!group->isChain()) { - ROS_ERROR_NAMED(LOGNAME, "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str()); + RCLCPP_ERROR(logger_robot_state, "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str()); return false; } if (!group->isLinkUpdated(link->getName())) { - ROS_ERROR_NAMED(LOGNAME, "Link name '%s' does not exist in the chain '%s' or is not a child for this chain", + RCLCPP_ERROR(logger_robot_state, "Link name '%s' does not exist in the chain '%s' or is not a child for this chain", link->getName().c_str(), group->getName().c_str()); return false; } @@ -1175,7 +1176,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link Eigen::Vector3d point_transform = link_transform * reference_point_position; /* - ROS_DEBUG_NAMED(LOGNAME, "Point from reference origin expressed in world coordinates: %f %f %f", + RCLCPP_DEBUG(logger_robot_state, "Point from reference origin expressed in world coordinates: %f %f %f", point_transform.x(), point_transform.y(), point_transform.z()); @@ -1187,11 +1188,11 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link while (link) { /* - ROS_DEBUG_NAMED(LOGNAME, "Link: %s, %f %f %f",link_state->getName().c_str(), + RCLCPP_DEBUG(logger_robot_state, "Link: %s, %f %f %f",link_state->getName().c_str(), link_state->getGlobalLinkTransform().translation().x(), link_state->getGlobalLinkTransform().translation().y(), link_state->getGlobalLinkTransform().translation().z()); - ROS_DEBUG_NAMED(LOGNAME, "Joint: %s",link_state->getParentJointState()->getName().c_str()); + RCLCPP_DEBUG(logger_robot_state, "Joint: %s",link_state->getParentJointState()->getName().c_str()); */ const JointModel* pjm = link->getParentJointModel(); if (pjm->getVariableCount() > 0) @@ -1229,7 +1230,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link jacobian.block<3, 1>(3, joint_index + 2) = jacobian.block<3, 1>(3, joint_index + 2) + joint_axis; } else - ROS_ERROR_NAMED(LOGNAME, "Unknown type of joint in Jacobian computation"); + RCLCPP_ERROR(logger_robot_state, "Unknown type of joint in Jacobian computation"); } if (pjm == root_joint_model) break; @@ -1259,7 +1260,7 @@ bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const Eigen::VectorXd return integrateVariableVelocity(jmg, qdot, dt, constraint); } -bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const geometry_msgs::Twist& twist, const std::string& tip, +bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const geometry_msgs::msg::Twist& twist, const std::string& tip, double dt, const GroupStateValidityCallbackFn& constraint) { Eigen::Matrix t; @@ -1334,13 +1335,13 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); if (!solver) { - ROS_ERROR_NAMED(LOGNAME, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(logger_robot_state, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint, options); } -bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, const std::string& tip, +bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg::Pose& pose, const std::string& tip, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { @@ -1357,7 +1358,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); if (!solver) { - ROS_ERROR_NAMED(LOGNAME, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(logger_robot_state, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } static std::vector consistency_limits; @@ -1375,7 +1376,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& namespace { bool ikCallbackFnAdapter(RobotState* state, const JointModelGroup* group, - const GroupStateValidityCallbackFn& constraint, const geometry_msgs::Pose& /*unused*/, + const GroupStateValidityCallbackFn& constraint, const geometry_msgs::msg::Pose& /*unused*/, const std::vector& ik_sol, moveit_msgs::msg::MoveItErrorCodes& error_code) { const std::vector& bij = group->getKinematicsSolverJointBijection(); @@ -1403,7 +1404,7 @@ bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& const LinkModel* lm = getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame); if (!lm) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "IK frame '" << ik_frame << "' does not exist."); + RCLCPP_ERROR(logger_robot_state, "The following IK frame does not exist:", ik_frame.c_str()); return false; } pose = getGlobalLinkTransform(lm).inverse() * pose; @@ -1447,7 +1448,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // Error check if (poses_in.size() != tips_in.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of poses must be the same as number of tips"); + RCLCPP_ERROR(logger_robot_state, "Number of poses must be the same as number of tips"); return false; } @@ -1466,7 +1467,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::string error_msg; if (!solver->supportsGroup(jmg, &error_msg)) { - ROS_ERROR_NAMED(LOGNAME, "Kinematics solver %s does not support joint group %s. Error: %s", + RCLCPP_ERROR(logger_robot_state, "Kinematics solver %s does not support joint group %s. Error: %s", typeid(*solver).name(), jmg->getName().c_str(), error_msg.c_str()); valid_solver = false; } @@ -1482,7 +1483,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is } else { - ROS_ERROR_NAMED(LOGNAME, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(logger_robot_state, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } } @@ -1491,7 +1492,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::vector consistency_limits; if (consistency_limit_sets.size() > 1) { - ROS_ERROR_NAMED(LOGNAME, "Invalid number (%zu) of sets of consistency limits for a setFromIK request " + RCLCPP_ERROR(logger_robot_state, "Invalid number (%zu) of sets of consistency limits for a setFromIK request " "that is being solved by a single IK solver", consistency_limit_sets.size()); return false; @@ -1505,7 +1506,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::vector tip_frames_used(solver_tip_frames.size(), false); // Create vector to hold the output frames in the same order as solver_tip_frames - std::vector ik_queries(solver_tip_frames.size()); + std::vector ik_queries(solver_tip_frames.size()); // Bring each pose to the frame of the IK solver for (std::size_t i = 0; i < poses_in.size(); ++i) @@ -1548,7 +1549,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is const EigenSTL::vector_Isometry3d& ab_trans = ab->getFixedTransforms(); if (ab_trans.size() != 1) { - ROS_ERROR_NAMED(LOGNAME, "Cannot use an attached body " + RCLCPP_ERROR(logger_robot_state, "Cannot use an attached body " "with multiple geometries as a reference frame."); return false; } @@ -1560,7 +1561,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is const robot_model::LinkModel* lm = getLinkModel(pose_frame); if (!lm) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Pose frame '" << pose_frame << "' does not exist."); + RCLCPP_ERROR(logger_robot_state, "The following Pose Frame does not exist ", pose_frame.c_str()); return false; } const robot_model::LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms(); @@ -1587,12 +1588,12 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // Make sure one of the tip frames worked if (!found_valid_frame) { - ROS_ERROR_NAMED(LOGNAME, "Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str()); + RCLCPP_ERROR(logger_robot_state, "Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str()); // Debug available tip frames std::stringstream ss; for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id) ss << solver_tip_frames[solver_tip_id] << ", "; - ROS_ERROR_NAMED(LOGNAME, "Available tip frames: [%s]", ss.str().c_str()); + RCLCPP_ERROR(logger_robot_state, "Available tip frames: [%s]", ss.str().c_str()); return false; } @@ -1600,7 +1601,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is tip_frames_used[solver_tip_id] = true; // Convert Eigen pose to geometry_msgs pose - geometry_msgs::Pose ik_query; + geometry_msgs::msg::Pose ik_query; ik_query = tf2::toMsg(pose); // Save into vectors @@ -1630,7 +1631,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is return false; // Convert Eigen pose to geometry_msgs pose - geometry_msgs::Pose ik_query; + geometry_msgs::msg::Pose ik_query; ik_query = tf2::toMsg(current_pose); // Save into vectors - but this needs to be ordered in the same order as the IK solver expects its tip frames @@ -1689,21 +1690,21 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: // Error check if (poses_in.size() != sub_groups.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(), + RCLCPP_ERROR(logger_robot_state, "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(), sub_groups.size()); return false; } if (tips_in.size() != sub_groups.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(), + RCLCPP_ERROR(logger_robot_state, "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(), sub_groups.size()); return false; } if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of consistency limit vectors must be the same as number of sub-groups"); + RCLCPP_ERROR(logger_robot_state, "Number of consistency limit vectors must be the same as number of sub-groups"); return false; } @@ -1711,7 +1712,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: { if (consistency_limits[i].size() != sub_groups[i]->getVariableCount()) { - ROS_ERROR_NAMED(LOGNAME, "Number of joints in consistency_limits is %zu but it should be should be %u", i, + RCLCPP_ERROR(logger_robot_state, "Number of joints in consistency_limits is %zu but it should be should be %u", i, sub_groups[i]->getVariableCount()); return false; } @@ -1724,7 +1725,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance(); if (!solver) { - ROS_ERROR_NAMED(LOGNAME, "Could not find solver for group '%s'", sub_groups[i]->getName().c_str()); + RCLCPP_ERROR(logger_robot_state, "Could not find solver for group '%s'", sub_groups[i]->getName().c_str()); return false; } solvers.push_back(solver); @@ -1760,7 +1761,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: const EigenSTL::vector_Isometry3d& ab_trans = ab->getFixedTransforms(); if (ab_trans.size() != 1) { - ROS_ERROR_NAMED(LOGNAME, "Cannot use an attached body with multiple geometries as a reference frame."); + RCLCPP_ERROR(logger_robot_state, "Cannot use an attached body with multiple geometries as a reference frame."); return false; } pose_frame = ab->getAttachedLinkName(); @@ -1784,14 +1785,14 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: if (pose_frame != solver_tip_frame) { - ROS_ERROR_NAMED(LOGNAME, "Cannot compute IK for query pose reference frame '%s', desired: '%s'", + RCLCPP_ERROR(logger_robot_state, "Cannot compute IK for query pose reference frame '%s', desired: '%s'", pose_frame.c_str(), solver_tip_frame.c_str()); return false; } } // Convert Eigen poses to geometry_msg format - std::vector ik_queries(poses_in.size()); + std::vector ik_queries(poses_in.size()); kinematics::KinematicsBase::IKCallbackFn ik_callback_fn; if (constraint) ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3); @@ -1868,7 +1869,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: copyJointGroupPositions(jmg, full_solution); if (constraint ? constraint(this, jmg, &full_solution[0]) : true) { - ROS_DEBUG_NAMED(LOGNAME, "Found IK solution"); + RCLCPP_DEBUG(logger_robot_state, "Found IK solution"); return true; } } @@ -1923,7 +1924,7 @@ double RobotState::computeCartesianPath(const JointModelGroup* group, std::vecto if (max_step.translation <= 0.0 && max_step.rotation <= 0.0) { - ROS_ERROR_NAMED(LOGNAME, + RCLCPP_ERROR(logger_robot_state, "Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and " "MaxEEFStep.translation components must be non-negative and at least one component must be " "greater than zero"); @@ -2055,7 +2056,7 @@ double RobotState::testRelativeJointSpaceJump(const JointModelGroup* group, std: { if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH) { - ROS_WARN_NAMED(LOGNAME, "The computed trajectory is too short to detect jumps in joint-space " + RCLCPP_WARN(logger_robot_state, "The computed trajectory is too short to detect jumps in joint-space " "Need at least %zu steps, only got %zu. Try a lower max_step.", MIN_STEPS_FOR_JUMP_THRESH, traj.size()); } @@ -2076,7 +2077,7 @@ double RobotState::testRelativeJointSpaceJump(const JointModelGroup* group, std: for (std::size_t i = 0; i < dist_vector.size(); ++i) if (dist_vector[i] > thres) { - ROS_DEBUG_NAMED(LOGNAME, "Truncating Cartesian path due to detected jump in joint-space distance"); + RCLCPP_DEBUG(logger_robot_state, "Truncating Cartesian path due to detected jump in joint-space distance"); percentage = (double)(i + 1) / (double)(traj.size()); traj.resize(i + 1); break; @@ -2111,7 +2112,7 @@ double RobotState::testAbsoluteJointSpaceJump(const JointModelGroup* group, std: type_index = 1; break; default: - ROS_WARN_NAMED(LOGNAME, "Joint %s has not supported type %s. \n" + RCLCPP_WARN(logger_robot_state, "Joint %s has not supported type %s. \n" "testAbsoluteJointSpaceJump only supports prismatic and revolute joints.", joint->getName().c_str(), joint->getTypeName().c_str()); continue; @@ -2122,7 +2123,7 @@ double RobotState::testAbsoluteJointSpaceJump(const JointModelGroup* group, std: double distance = traj[traj_ix]->distance(*traj[traj_ix + 1], joint); if (distance > data[type_index].limit_) { - ROS_DEBUG_NAMED(LOGNAME, "Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s", distance, + RCLCPP_DEBUG(logger_robot_state, "Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s", distance, data[type_index].limit_, joint->getName().c_str()); still_valid = false; break; diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index 9f13d7d2f7..a0786432ca 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -52,7 +52,7 @@ #endif #if VISUALIZE_PR2_RVIZ -#include +#include "rclcpp/rclcpp.hpp" #include #include #endif @@ -151,32 +151,34 @@ TEST_F(TestAABB, TestPR2) // Initialize a ROS publisher char* argv[0]; int argc = 0; - ros::init(argc, argv, "visualize_pr2"); - ros::NodeHandle nh; - ros::Publisher pub_aabb = nh.advertise("/visualization_aabb", 10); - ros::Publisher pub_obb = nh.advertise("/visualization_obb", 10); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("visualize_pr2"); + auto pub_aabb = node->create_publisher("/visualization_aabb", rmw_qos_profile_default); + auto pub_obb = node->create_publisher("/visualization_obb", rmw_qos_profile_default); + rclcpp::Rate loop_rate(10); // Wait for the publishers to establish connections sleep(5); // Prepare the ROS message we will reuse throughout the rest of the function. - visualization_msgs::Marker msg; - msg.header.frame_id = pr2_state.getRobotModel()->getRootLinkName(); - msg.type = visualization_msgs::Marker::CUBE; - msg.color.a = 0.5; - msg.lifetime.sec = 3000; + auto msg = std::make_shared(); + + msg->header.frame_id = pr2_state.getRobotModel()->getRootLinkName(); + msg->type = visualization_msgs::Marker::CUBE; + msg->color.a = 0.5; + msg->lifetime.sec = 3000; // Publish the AABB of the whole model - msg.ns = "pr2"; - msg.pose.position.x = (pr2_aabb[0] + pr2_aabb[1]) / 2; - msg.pose.position.y = (pr2_aabb[2] + pr2_aabb[3]) / 2; - msg.pose.position.z = (pr2_aabb[4] + pr2_aabb[5]) / 2; - msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0; - msg.pose.orientation.w = 1; - msg.scale.x = pr2_aabb[1] - pr2_aabb[0]; - msg.scale.y = pr2_aabb[3] - pr2_aabb[2]; - msg.scale.z = pr2_aabb[5] - pr2_aabb[4]; - pub_aabb.publish(msg); + msg->ns = "pr2"; + msg->pose.position.x = (pr2_aabb[0] + pr2_aabb[1]) / 2; + msg->pose.position.y = (pr2_aabb[2] + pr2_aabb[3]) / 2; + msg->pose.position.z = (pr2_aabb[4] + pr2_aabb[5]) / 2; + msg->pose.orientation.x = msg->pose.orientation.y = msg->pose.orientation.z = 0; + msg->pose.orientation.w = 1; + msg->scale.x = pr2_aabb[1] - pr2_aabb[0]; + msg->scale.y = pr2_aabb[3] - pr2_aabb[2]; + msg->scale.z = pr2_aabb[5] - pr2_aabb[4]; + pub_aabb->publish(msg); // Publish BBs for all links std::vector links = pr2_state.getRobotModel()->getLinkModelsWithCollisionGeometry(); @@ -189,35 +191,35 @@ TEST_F(TestAABB, TestPR2) aabb.extendWithTransformedBox(transform, extents); // Publish AABB - msg.ns = links[i]->getName(); - msg.pose.position.x = transform.translation()[0]; - msg.pose.position.y = transform.translation()[1]; - msg.pose.position.z = transform.translation()[2]; - msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0; - msg.pose.orientation.w = 1; - msg.color.r = 1; - msg.color.b = 0; - msg.scale.x = aabb.sizes()[0]; - msg.scale.y = aabb.sizes()[1]; - msg.scale.z = aabb.sizes()[2]; - pub_aabb.publish(msg); + msg->ns = links[i]->getName(); + msg->pose.position.x = transform.translation()[0]; + msg->pose.position.y = transform.translation()[1]; + msg->pose.position.z = transform.translation()[2]; + msg->pose.orientation.x = msg->pose.orientation.y = msg->pose.orientation.z = 0; + msg->pose.orientation.w = 1; + msg->color.r = 1; + msg->color.b = 0; + msg->scale.x = aabb.sizes()[0]; + msg->scale.y = aabb.sizes()[1]; + msg->scale.z = aabb.sizes()[2]; + pub_aabb->publish(msg); // Publish OBB (oriented BB) - msg.ns += "-obb"; - msg.pose.position.x = transform.translation()[0]; - msg.pose.position.y = transform.translation()[1]; - msg.pose.position.z = transform.translation()[2]; - msg.scale.x = extents[0]; - msg.scale.y = extents[1]; - msg.scale.z = extents[2]; - msg.color.r = 0; - msg.color.b = 1; + msg->ns += "-obb"; + msg->pose.position.x = transform.translation()[0]; + msg->pose.position.y = transform.translation()[1]; + msg->pose.position.z = transform.translation()[2]; + msg->scale.x = extents[0]; + msg->scale.y = extents[1]; + msg->scale.z = extents[2]; + msg->color.r = 0; + msg->color.b = 1; Eigen::Quaterniond q(transform.rotation()); - msg.pose.orientation.x = q.x(); - msg.pose.orientation.y = q.y(); - msg.pose.orientation.z = q.z(); - msg.pose.orientation.w = q.w(); - pub_obb.publish(msg); + msg->pose.orientation.x = q.x(); + msg->pose.orientation.y = q.y(); + msg->pose.orientation.z = q.z(); + msg->pose.orientation.w = q.w(); + pub_obb->publish(msg); } // Publish BBs for all attached bodies @@ -235,35 +237,35 @@ TEST_F(TestAABB, TestPR2) aabb.extendWithTransformedBox(transforms[i], extents); // Publish AABB - msg.ns = (*it)->getName() + boost::lexical_cast(i); - msg.pose.position.x = transforms[i].translation()[0]; - msg.pose.position.y = transforms[i].translation()[1]; - msg.pose.position.z = transforms[i].translation()[2]; - msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0; - msg.pose.orientation.w = 1; - msg.color.r = 1; - msg.color.b = 0; - msg.scale.x = aabb.sizes()[0]; - msg.scale.y = aabb.sizes()[1]; - msg.scale.z = aabb.sizes()[2]; - pub_aabb.publish(msg); + msg->ns = (*it)->getName() + boost::lexical_cast(i); + msg->pose.position.x = transforms[i].translation()[0]; + msg->pose.position.y = transforms[i].translation()[1]; + msg->pose.position.z = transforms[i].translation()[2]; + msg->pose.orientation.x = msg->pose.orientation.y = msg->pose.orientation.z = 0; + msg->pose.orientation.w = 1; + msg->color.r = 1; + msg->color.b = 0; + msg->scale.x = aabb.sizes()[0]; + msg->scale.y = aabb.sizes()[1]; + msg->scale.z = aabb.sizes()[2]; + pub_aabb->publish(msg); // Publish OBB (oriented BB) - msg.ns += "-obb"; - msg.pose.position.x = transforms[i].translation()[0]; - msg.pose.position.y = transforms[i].translation()[1]; - msg.pose.position.z = transforms[i].translation()[2]; - msg.scale.x = extents[0]; - msg.scale.y = extents[1]; - msg.scale.z = extents[2]; - msg.color.r = 0; - msg.color.b = 1; + msg->ns += "-obb"; + msg->pose.position.x = transforms[i].translation()[0]; + msg->pose.position.y = transforms[i].translation()[1]; + msg->pose.position.z = transforms[i].translation()[2]; + msg->scale.x = extents[0]; + msg->scale.y = extents[1]; + msg->scale.z = extents[2]; + msg->color.r = 0; + msg->color.b = 1; Eigen::Quaterniond q(transforms[i].rotation()); - msg.pose.orientation.x = q.x(); - msg.pose.orientation.y = q.y(); - msg.pose.orientation.z = q.z(); - msg.pose.orientation.w = q.w(); - pub_obb.publish(msg); + msg->pose.orientation.x = q.x(); + msg->pose.orientation.y = q.y(); + msg->pose.orientation.z = q.z(); + msg->pose.orientation.w = q.w(); + pub_obb->publish(msg); } } #endif From ab2c7285234a97694b2c5b1a54924fb3b82b50e6 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 16 May 2019 12:36:26 +0200 Subject: [PATCH 02/13] Fixing Logger Co-authored-by: vmayoral Co-authored-by: anasarrak --- moveit_core/robot_state/src/conversions.cpp | 44 ++++++----- moveit_core/robot_state/src/robot_state.cpp | 84 +++++++++++---------- 2 files changed, 67 insertions(+), 61 deletions(-) diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index a87c75c22b..943c80b82a 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include "rclcpp/rclcpp.hpp" namespace moveit { @@ -52,11 +52,14 @@ namespace core namespace { +// Logger +rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("robot_state"); + static bool _jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, RobotState& state) { if (joint_state.name.size() != joint_state.position.size()) { - RCLCPP_ERROR(logger_robot_state, "Different number of names and positions in JointState message: %zu, %zu", + RCLCPP_ERROR(LOGGER, "Different number of names and positions in JointState message: %zu, %zu", joint_state.name.size(), joint_state.position.size()); return false; } @@ -72,7 +75,7 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::msg::MultiDOFJointSta std::size_t nj = mjs.joint_names.size(); if (nj != mjs.transforms.size()) { - RCLCPP_ERROR(logger_robot_state, "Different number of names, values or frames in MultiDOFJointState message."); + RCLCPP_ERROR(LOGGER, "Different number of names, values or frames in MultiDOFJointState message."); return false; } @@ -94,14 +97,14 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::msg::MultiDOFJointSta } catch (std::exception& ex) { - RCLCPP_ERROR(logger_robot_state, "Caught %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Caught %s", ex.what()); error = true; } else error = true; if (error) - RCLCPP_WARN(logger_robot_state, "The transform for multi-dof joints was specified in frame '%s' " + RCLCPP_WARN(LOGGER, "The transform for multi-dof joints was specified in frame '%s' " "but it was not possible to transform that to frame '%s'", mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str()); } @@ -111,7 +114,7 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::msg::MultiDOFJointSta const std::string& joint_name = mjs.joint_names[i]; if (!state.getRobotModel()->hasJointModel(joint_name)) { - RCLCPP_WARN(logger_robot_state, "No joint matching multi-dof joint '%s'", joint_name.c_str()); + RCLCPP_WARN(LOGGER, "No joint matching multi-dof joint '%s'", joint_name.c_str()); error = true; continue; } @@ -226,20 +229,20 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att { if (aco.object.primitives.size() != aco.object.primitive_poses.size()) { - RCLCPP_ERROR(logger_robot_state, "Number of primitive shapes does not match " + RCLCPP_ERROR(LOGGER, "Number of primitive shapes does not match " "number of poses in collision object message"); return; } if (aco.object.meshes.size() != aco.object.mesh_poses.size()) { - RCLCPP_ERROR(logger_robot_state, "Number of meshes does not match number of poses in collision object message"); + RCLCPP_ERROR(LOGGER, "Number of meshes does not match number of poses in collision object message"); return; } if (aco.object.planes.size() != aco.object.plane_poses.size()) { - RCLCPP_ERROR(logger_robot_state, "Number of planes does not match number of poses in collision object message"); + RCLCPP_ERROR(LOGGER, "Number of planes does not match number of poses in collision object message"); return; } @@ -295,7 +298,7 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att else { t0.setIdentity(); - RCLCPP_ERROR(logger_robot_state, "Cannot properly transform from frame '%s'. " + RCLCPP_ERROR(LOGGER, "Cannot properly transform from frame '%s'. " "The pose of the attached body may be incorrect", aco.object.header.frame_id.c_str()); } @@ -305,31 +308,31 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att } if (shapes.empty()) { - RCLCPP_ERROR(logger_robot_state, "There is no geometry to attach to link '%s' as part of attached body '%s'", + RCLCPP_ERROR(LOGGER, "There is no geometry to attach to link '%s' as part of attached body '%s'", aco.link_name.c_str(), aco.object.id.c_str()); } else { if (state.clearAttachedBody(aco.object.id)) - RCLCPP_DEBUG(logger_robot_state, "The robot state already had an object named '%s' attached to link '%s'. " + RCLCPP_DEBUG(LOGGER, "The robot state already had an object named '%s' attached to link '%s'. " "The object was replaced.", aco.object.id.c_str(), aco.link_name.c_str()); state.attachBody(aco.object.id, shapes, poses, aco.touch_links, aco.link_name, aco.detach_posture); - RCLCPP_DEBUG(logger_robot_state, "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str()); + RCLCPP_DEBUG(LOGGER, "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str()); } } } else - RCLCPP_ERROR(logger_robot_state, "The attached body for link '%s' has no geometry", aco.link_name.c_str()); + RCLCPP_ERROR(LOGGER, "The attached body for link '%s' has no geometry", aco.link_name.c_str()); } else if (aco.object.operation == moveit_msgs::msg::CollisionObject::REMOVE) { if (!state.clearAttachedBody(aco.object.id)) - RCLCPP_ERROR(logger_robot_state, "The attached body '%s' can not be removed because it does not exist", + RCLCPP_ERROR(LOGGER, "The attached body '%s' can not be removed because it does not exist", aco.link_name.c_str()); } else - RCLCPP_ERROR(logger_robot_state, "Unknown collision object operation: %d", aco.object.operation); + RCLCPP_ERROR(LOGGER, "Unknown collision object operation: %d", aco.object.operation); } static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::msg::RobotState& robot_state, @@ -340,7 +343,7 @@ static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_ if (!rs.is_diff && rs.joint_state.name.empty() && rs.multi_dof_joint_state.joint_names.empty()) { - RCLCPP_ERROR(logger_robot_state, "Found empty JointState message"); + RCLCPP_ERROR(LOGGER, "Found empty JointState message"); return false; } @@ -436,12 +439,12 @@ bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& tra { if (trajectory.points.empty() || point_id > trajectory.points.size() - 1) { - RCLCPP_ERROR(logger_robot_state, "Invalid point_id"); + RCLCPP_ERROR(LOGGER, "Invalid point_id"); return false; } if (trajectory.joint_names.empty()) { - RCLCPP_ERROR(logger_robot_state, "No joint names specified"); + RCLCPP_ERROR(LOGGER, "No joint names specified"); return false; } @@ -531,7 +534,8 @@ void streamToRobotState(RobotState& state, const std::string& line, const std::s { // Get a variable if (!std::getline(line_stream, cell, separator[0])) - RCLCPP_ERROR(logger_robot_state, "Missing variable " , state.getVariableNames()[i].c_str()); + // ROS_ERROR_STREAM_NAMED(LOGNAME.c_str(), "Missing variable " << state.getVariableNames()[i]); + RCLCPP_ERROR(LOGGER, "Missing variable " , state.getVariableNames()[i].c_str()); printf("show variablename %s\n", state.getVariableNames()[i].c_str()); state.getVariablePositions()[i] = boost::lexical_cast(cell.c_str()); } diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 68dba0a949..106a13472b 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -44,13 +44,15 @@ #include #include #include -#include #include "rclcpp/rclcpp.hpp" namespace moveit { namespace core { +// Logger +rclcpp::Logger LOGGER = rclcpp::get_logger("robot_state"); + /** \brief It is recommended that there are at least 10 steps per trajectory * for testing jump thresholds with computeCartesianPath. With less than 10 steps * it is difficult to choose a jump_threshold parameter that effectively separates @@ -182,7 +184,7 @@ bool RobotState::checkJointTransforms(const JointModel* joint) const { if (dirtyJointTransform(joint)) { - RCLCPP_WARN(logger_robot_state, "Returning dirty joint transforms for joint '%s'", joint->getName().c_str()); + RCLCPP_WARN(LOGGER, "Returning dirty joint transforms for joint '%s'", joint->getName().c_str()); return false; } return true; @@ -192,7 +194,7 @@ bool RobotState::checkLinkTransforms() const { if (dirtyLinkTransforms()) { - RCLCPP_WARN(logger_robot_state, "Returning dirty link transforms"); + RCLCPP_WARN(LOGGER, "Returning dirty link transforms"); return false; } return true; @@ -202,7 +204,7 @@ bool RobotState::checkCollisionTransforms() const { if (dirtyCollisionBodyTransforms()) { - RCLCPP_WARN(logger_robot_state, "Returning dirty collision body transforms"); + RCLCPP_WARN(LOGGER, "Returning dirty collision body transforms"); return false; } return true; @@ -451,7 +453,7 @@ void RobotState::setJointEfforts(const JointModel* joint, const double* effort) { if (has_acceleration_) { - RCLCPP_ERROR(logger_robot_state, "Unable to set joint efforts because array is being used for accelerations"); + RCLCPP_ERROR(LOGGER, "Unable to set joint efforts because array is being used for accelerations"); return; } has_effort_ = true; @@ -880,7 +882,7 @@ const AttachedBody* RobotState::getAttachedBody(const std::string& id) const std::map::const_iterator it = attached_body_map_.find(id); if (it == attached_body_map_.end()) { - RCLCPP_ERROR(logger_robot_state, "Attached body '%s' not found", id.c_str()); + RCLCPP_ERROR(LOGGER, "Attached body '%s' not found", id.c_str()); return nullptr; } else @@ -1021,7 +1023,7 @@ const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& id) co std::map::const_iterator jt = attached_body_map_.find(id); if (jt == attached_body_map_.end()) { - RCLCPP_ERROR(logger_robot_state, "Transform from frame '%s' to frame '%s' is not known " + RCLCPP_ERROR(LOGGER, "Transform from frame '%s' to frame '%s' is not known " "('%s' should be a link name or an attached body id).", id.c_str(), robot_model_->getModelFrame().c_str(), id.c_str()); return IDENTITY_TRANSFORM; @@ -1029,12 +1031,12 @@ const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& id) co const EigenSTL::vector_Isometry3d& tf = jt->second->getGlobalCollisionBodyTransforms(); if (tf.empty()) { - RCLCPP_ERROR(logger_robot_state, "Attached body '%s' has no geometry associated to it. No transform to return.", + RCLCPP_ERROR(LOGGER, "Attached body '%s' has no geometry associated to it. No transform to return.", id.c_str()); return IDENTITY_TRANSFORM; } if (tf.size() > 1) - RCLCPP_DEBUG(logger_robot_state, "There are multiple geometries associated to attached body '%s'. " + RCLCPP_DEBUG(LOGGER, "There are multiple geometries associated to attached body '%s'. " "Returning the transform for the first one.", id.c_str()); return tf[0]; @@ -1073,7 +1075,7 @@ void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, cons for (std::size_t i = 0; i < link_names.size(); ++i) { - RCLCPP_DEBUG(logger_robot_state, "Trying to get marker for link '%s'", link_names[i].c_str()); + RCLCPP_DEBUG(LOGGER, "Trying to get marker for link '%s'", link_names[i].c_str()); const LinkModel* lm = robot_model_->getLinkModel(link_names[i]); if (!lm) continue; @@ -1153,13 +1155,13 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link if (!group->isChain()) { - RCLCPP_ERROR(logger_robot_state, "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str()); + RCLCPP_ERROR(LOGGER, "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str()); return false; } if (!group->isLinkUpdated(link->getName())) { - RCLCPP_ERROR(logger_robot_state, "Link name '%s' does not exist in the chain '%s' or is not a child for this chain", + RCLCPP_ERROR(LOGGER, "Link name '%s' does not exist in the chain '%s' or is not a child for this chain", link->getName().c_str(), group->getName().c_str()); return false; } @@ -1176,7 +1178,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link Eigen::Vector3d point_transform = link_transform * reference_point_position; /* - RCLCPP_DEBUG(logger_robot_state, "Point from reference origin expressed in world coordinates: %f %f %f", + RCLCPP_DEBUG(LOGGER, "Point from reference origin expressed in world coordinates: %f %f %f", point_transform.x(), point_transform.y(), point_transform.z()); @@ -1188,11 +1190,11 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link while (link) { /* - RCLCPP_DEBUG(logger_robot_state, "Link: %s, %f %f %f",link_state->getName().c_str(), + RCLCPP_DEBUG(LOGGER, "Link: %s, %f %f %f",link_state->getName().c_str(), link_state->getGlobalLinkTransform().translation().x(), link_state->getGlobalLinkTransform().translation().y(), link_state->getGlobalLinkTransform().translation().z()); - RCLCPP_DEBUG(logger_robot_state, "Joint: %s",link_state->getParentJointState()->getName().c_str()); + RCLCPP_DEBUG(LOGGER, "Joint: %s",link_state->getParentJointState()->getName().c_str()); */ const JointModel* pjm = link->getParentJointModel(); if (pjm->getVariableCount() > 0) @@ -1230,7 +1232,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link jacobian.block<3, 1>(3, joint_index + 2) = jacobian.block<3, 1>(3, joint_index + 2) + joint_axis; } else - RCLCPP_ERROR(logger_robot_state, "Unknown type of joint in Jacobian computation"); + RCLCPP_ERROR(LOGGER, "Unknown type of joint in Jacobian computation"); } if (pjm == root_joint_model) break; @@ -1335,7 +1337,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); if (!solver) { - RCLCPP_ERROR(logger_robot_state, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(LOGGER, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint, options); @@ -1358,7 +1360,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); if (!solver) { - RCLCPP_ERROR(logger_robot_state, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(LOGGER, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } static std::vector consistency_limits; @@ -1404,7 +1406,7 @@ bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& const LinkModel* lm = getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame); if (!lm) { - RCLCPP_ERROR(logger_robot_state, "The following IK frame does not exist:", ik_frame.c_str()); + RCLCPP_ERROR(LOGGER, "The following IK frame does not exist:", ik_frame.c_str()); return false; } pose = getGlobalLinkTransform(lm).inverse() * pose; @@ -1448,7 +1450,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // Error check if (poses_in.size() != tips_in.size()) { - RCLCPP_ERROR(logger_robot_state, "Number of poses must be the same as number of tips"); + RCLCPP_ERROR(LOGGER, "Number of poses must be the same as number of tips"); return false; } @@ -1467,7 +1469,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::string error_msg; if (!solver->supportsGroup(jmg, &error_msg)) { - RCLCPP_ERROR(logger_robot_state, "Kinematics solver %s does not support joint group %s. Error: %s", + RCLCPP_ERROR(LOGGER, "Kinematics solver %s does not support joint group %s. Error: %s", typeid(*solver).name(), jmg->getName().c_str(), error_msg.c_str()); valid_solver = false; } @@ -1483,7 +1485,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is } else { - RCLCPP_ERROR(logger_robot_state, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(LOGGER, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } } @@ -1492,7 +1494,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::vector consistency_limits; if (consistency_limit_sets.size() > 1) { - RCLCPP_ERROR(logger_robot_state, "Invalid number (%zu) of sets of consistency limits for a setFromIK request " + RCLCPP_ERROR(LOGGER, "Invalid number (%zu) of sets of consistency limits for a setFromIK request " "that is being solved by a single IK solver", consistency_limit_sets.size()); return false; @@ -1549,7 +1551,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is const EigenSTL::vector_Isometry3d& ab_trans = ab->getFixedTransforms(); if (ab_trans.size() != 1) { - RCLCPP_ERROR(logger_robot_state, "Cannot use an attached body " + RCLCPP_ERROR(LOGGER, "Cannot use an attached body " "with multiple geometries as a reference frame."); return false; } @@ -1561,7 +1563,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is const robot_model::LinkModel* lm = getLinkModel(pose_frame); if (!lm) { - RCLCPP_ERROR(logger_robot_state, "The following Pose Frame does not exist ", pose_frame.c_str()); + RCLCPP_ERROR(LOGGER, "The following Pose Frame does not exist ", pose_frame.c_str()); return false; } const robot_model::LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms(); @@ -1588,12 +1590,12 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // Make sure one of the tip frames worked if (!found_valid_frame) { - RCLCPP_ERROR(logger_robot_state, "Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str()); + RCLCPP_ERROR(LOGGER, "Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str()); // Debug available tip frames std::stringstream ss; for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id) ss << solver_tip_frames[solver_tip_id] << ", "; - RCLCPP_ERROR(logger_robot_state, "Available tip frames: [%s]", ss.str().c_str()); + RCLCPP_ERROR(LOGGER, "Available tip frames: [%s]", ss.str().c_str()); return false; } @@ -1690,21 +1692,21 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: // Error check if (poses_in.size() != sub_groups.size()) { - RCLCPP_ERROR(logger_robot_state, "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(), + RCLCPP_ERROR(LOGGER, "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(), sub_groups.size()); return false; } if (tips_in.size() != sub_groups.size()) { - RCLCPP_ERROR(logger_robot_state, "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(), + RCLCPP_ERROR(LOGGER, "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(), sub_groups.size()); return false; } if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size()) { - RCLCPP_ERROR(logger_robot_state, "Number of consistency limit vectors must be the same as number of sub-groups"); + RCLCPP_ERROR(LOGGER, "Number of consistency limit vectors must be the same as number of sub-groups"); return false; } @@ -1712,7 +1714,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: { if (consistency_limits[i].size() != sub_groups[i]->getVariableCount()) { - RCLCPP_ERROR(logger_robot_state, "Number of joints in consistency_limits is %zu but it should be should be %u", i, + RCLCPP_ERROR(LOGGER, "Number of joints in consistency_limits is %zu but it should be should be %u", i, sub_groups[i]->getVariableCount()); return false; } @@ -1725,7 +1727,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance(); if (!solver) { - RCLCPP_ERROR(logger_robot_state, "Could not find solver for group '%s'", sub_groups[i]->getName().c_str()); + RCLCPP_ERROR(LOGGER, "Could not find solver for group '%s'", sub_groups[i]->getName().c_str()); return false; } solvers.push_back(solver); @@ -1761,7 +1763,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: const EigenSTL::vector_Isometry3d& ab_trans = ab->getFixedTransforms(); if (ab_trans.size() != 1) { - RCLCPP_ERROR(logger_robot_state, "Cannot use an attached body with multiple geometries as a reference frame."); + RCLCPP_ERROR(LOGGER, "Cannot use an attached body with multiple geometries as a reference frame."); return false; } pose_frame = ab->getAttachedLinkName(); @@ -1785,7 +1787,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: if (pose_frame != solver_tip_frame) { - RCLCPP_ERROR(logger_robot_state, "Cannot compute IK for query pose reference frame '%s', desired: '%s'", + RCLCPP_ERROR(LOGGER, "Cannot compute IK for query pose reference frame '%s', desired: '%s'", pose_frame.c_str(), solver_tip_frame.c_str()); return false; } @@ -1821,7 +1823,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: do { ++attempts; - ROS_DEBUG_NAMED(LOGNAME, "IK attempt: %d", attempts); + RCLCPP_DEBUG(LOGGER, "IK attempt: %d", attempts); bool found_solution = true; for (std::size_t sg = 0; sg < sub_groups.size(); ++sg) { @@ -1869,7 +1871,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: copyJointGroupPositions(jmg, full_solution); if (constraint ? constraint(this, jmg, &full_solution[0]) : true) { - RCLCPP_DEBUG(logger_robot_state, "Found IK solution"); + RCLCPP_DEBUG(LOGGER, "Found IK solution"); return true; } } @@ -1924,7 +1926,7 @@ double RobotState::computeCartesianPath(const JointModelGroup* group, std::vecto if (max_step.translation <= 0.0 && max_step.rotation <= 0.0) { - RCLCPP_ERROR(logger_robot_state, + RCLCPP_ERROR(LOGGER, "Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and " "MaxEEFStep.translation components must be non-negative and at least one component must be " "greater than zero"); @@ -2056,7 +2058,7 @@ double RobotState::testRelativeJointSpaceJump(const JointModelGroup* group, std: { if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH) { - RCLCPP_WARN(logger_robot_state, "The computed trajectory is too short to detect jumps in joint-space " + RCLCPP_WARN(LOGGER, "The computed trajectory is too short to detect jumps in joint-space " "Need at least %zu steps, only got %zu. Try a lower max_step.", MIN_STEPS_FOR_JUMP_THRESH, traj.size()); } @@ -2077,7 +2079,7 @@ double RobotState::testRelativeJointSpaceJump(const JointModelGroup* group, std: for (std::size_t i = 0; i < dist_vector.size(); ++i) if (dist_vector[i] > thres) { - RCLCPP_DEBUG(logger_robot_state, "Truncating Cartesian path due to detected jump in joint-space distance"); + RCLCPP_DEBUG(LOGGER, "Truncating Cartesian path due to detected jump in joint-space distance"); percentage = (double)(i + 1) / (double)(traj.size()); traj.resize(i + 1); break; @@ -2112,7 +2114,7 @@ double RobotState::testAbsoluteJointSpaceJump(const JointModelGroup* group, std: type_index = 1; break; default: - RCLCPP_WARN(logger_robot_state, "Joint %s has not supported type %s. \n" + RCLCPP_WARN(LOGGER, "Joint %s has not supported type %s. \n" "testAbsoluteJointSpaceJump only supports prismatic and revolute joints.", joint->getName().c_str(), joint->getTypeName().c_str()); continue; @@ -2123,7 +2125,7 @@ double RobotState::testAbsoluteJointSpaceJump(const JointModelGroup* group, std: double distance = traj[traj_ix]->distance(*traj[traj_ix + 1], joint); if (distance > data[type_index].limit_) { - RCLCPP_DEBUG(logger_robot_state, "Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s", distance, + RCLCPP_DEBUG(LOGGER, "Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s", distance, data[type_index].limit_, joint->getName().c_str()); still_valid = false; break; From 11701fea4af9c59cbc1ffbc76018d61b5fa2e9f5 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 16 May 2019 12:36:58 +0200 Subject: [PATCH 03/13] robot_state - fixing tests --- .../test/robot_state_benchmark.cpp | 4 +-- .../robot_state/test/robot_state_test.cpp | 15 ++++++--- moveit_core/robot_state/test/test_aabb.cpp | 32 +++++++++++++------ .../test/test_kinematic_complex.cpp | 2 +- 4 files changed, 37 insertions(+), 16 deletions(-) diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index cd4bdbf993..ee2ea149ba 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -38,8 +38,8 @@ #include #include #include -#include #include +#include // Helper class to measure time within a scoped block and output the result class ScopedTimer @@ -96,7 +96,7 @@ class Timing : public testing::Test TEST_F(Timing, stateUpdate) { - robot_model::RobotModelPtr model = moveit::core::loadTestingRobotModel("pr2_description"); + robot_model::RobotModelPtr model = moveit::core::loadTestingRobotModel("pr2"); ASSERT_TRUE(bool(model)); robot_state::RobotState state(model); ScopedTimer t("RobotState updates: "); diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 555847f9f7..836976d00a 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -116,17 +116,24 @@ TEST(Loading, SimpleRobot) TEST(LoadingAndFK, SimpleRobot) { moveit::core::RobotModelBuilder builder("myrobot", "base_link"); - geometry_msgs::Pose pose; - tf2::toMsg(tf2::Vector3(-0.1, 0, 0), pose.position); + geometry_msgs::msg::Pose pose; + pose.position.x = -0.1; + pose.position.y = 0; + pose.position.z = 0; + tf2::Quaternion q; q.setRPY(0, 0, -1); pose.orientation = tf2::toMsg(q); builder.addCollisionBox("base_link", { 1, 2, 1 }, pose); - tf2::toMsg(tf2::Vector3(0, 0, 0), pose.position); + pose.position.x = 0; + pose.position.y = 0; + pose.position.z = 0; q.setRPY(0, 0, 0); pose.orientation = tf2::toMsg(q); builder.addVisualBox("base_link", { 1, 2, 1 }, pose); - tf2::toMsg(tf2::Vector3(0, 0.099, 0), pose.position); + pose.position.x = 0; + pose.position.y = 0.099; + pose.position.z = 0; q.setRPY(0, 0, 0); pose.orientation = tf2::toMsg(q); builder.addInertial("base_link", 2.81, pose, 0.1, -0.2, 0.5, -0.09, 1, 0.101); diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index a0786432ca..e7987e57e4 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -275,16 +275,22 @@ TEST_F(TestAABB, TestSimple) { // Contains a link with simple geometry and an offset in the collision link moveit::core::RobotModelBuilder builder("simple", "base_footprint"); - geometry_msgs::Pose origin; - tf2::toMsg(tf2::Vector3(0, 0, 0.051), origin.position); + geometry_msgs::msg::Pose origin; + origin.position.x = 0; + origin.position.y = 0; + origin.position.z = 0.051; origin.orientation.w = 1.0; builder.addChain("base_footprint->base_link", "fixed", { origin }); - tf2::toMsg(tf2::Vector3(0, 0, 0), origin.position); + origin.position.x = 0; + origin.position.y = 0; + origin.position.z = 0; builder.addCollisionMesh("base_link", "package://moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl", origin); - tf2::toMsg(tf2::Vector3(0, 0, 0.071), origin.position); + origin.position.x = 0; + origin.position.y = 0; + origin.position.z = 0.071; builder.addCollisionBox("base_footprint", { 0.001, 0.001, 0.001 }, origin); builder.addVirtualJoint("odom_combined", "base_footprint", "planar", "world_joint"); @@ -308,17 +314,25 @@ TEST_F(TestAABB, TestComplex) { // Contains a link with simple geometry and an offset and rotation in the collision link moveit::core::RobotModelBuilder builder("complex", "base_footprint"); - geometry_msgs::Pose origin; - tf2::toMsg(tf2::Vector3(0, 0, 1.0), origin.position); + geometry_msgs::msg::Pose origin; + origin.position.x = 0; + origin.position.y = 0; + origin.position.z = 1.0; tf2::Quaternion q; q.setRPY(0, 0, 1.5708); origin.orientation = tf2::toMsg(q); builder.addChain("base_footprint->base_link", "fixed", { origin }); - tf2::toMsg(tf2::Vector3(5.0, 0, 1.0), origin.position); + origin.position.x = 5.0; + origin.position.y = 0; + origin.position.z = 1.0; builder.addCollisionBox("base_link", { 1.0, 0.1, 0.1 }, origin); - tf2::toMsg(tf2::Vector3(4.0, 0, 1.0), origin.position); + origin.position.x = 4.0; + origin.position.y = 0; + origin.position.z = 1.0; builder.addCollisionBox("base_link", { 1.0, 0.1, 0.1 }, origin); - tf2::toMsg(tf2::Vector3(-5.0, 0.0, -1.0), origin.position); + origin.position.x = -5.0; + origin.position.y = 0; + origin.position.z = -1.0; q.setRPY(0, 1.5708, 0); origin.orientation = tf2::toMsg(q); builder.addCollisionBox("base_footprint", { 0.1, 1.0, 0.1 }, origin); diff --git a/moveit_core/robot_state/test/test_kinematic_complex.cpp b/moveit_core/robot_state/test/test_kinematic_complex.cpp index 9fc0926834..9febb7207a 100644 --- a/moveit_core/robot_state/test/test_kinematic_complex.cpp +++ b/moveit_core/robot_state/test/test_kinematic_complex.cpp @@ -251,7 +251,7 @@ TEST_F(LoadPlanningModelsPr2, FullTest) poses.push_back(Eigen::Isometry3d::Identity()); std::set touch_links; - trajectory_msgs::JointTrajectory empty_state; + trajectory_msgs::msg::JointTrajectory empty_state; moveit::core::AttachedBody* attached_body = new moveit::core::AttachedBody( robot_model->getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state); ks.attachBody(attached_body); From 61cf61407ee69a132847e8ca099caa8d7484d234 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 16 May 2019 12:37:23 +0200 Subject: [PATCH 04/13] robot_state - Fixing CMakeLists.txt --- moveit_core/robot_state/CMakeLists.txt | 135 ++++++++++++++++++++++--- 1 file changed, 119 insertions(+), 16 deletions(-) diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 59310d3e8d..f63131f910 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -1,36 +1,139 @@ set(MOVEIT_LIB_NAME moveit_robot_state) -add_library(${MOVEIT_LIB_NAME} +add_library(${MOVEIT_LIB_NAME} SHARED src/attached_body.cpp src/conversions.cpp src/robot_state.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) - -target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model moveit_kinematics_base moveit_transforms ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) - -# add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${MOVEIT_LIB_NAME} + moveit_robot_model + moveit_kinematics_base + moveit_transforms + ${urdfdom_LIBRARIES} + ${geometric_shapes_LIBRARIES} + ${urdfdom_headers_LIBRARIES} + ${Boost_LIBRARIES}) +ament_target_dependencies(${MOVEIT_LIB_NAME} + moveit_robot_model + moveit_kinematics_base + moveit_transforms + urdf + tf2_geometry_msgs +) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) install(DIRECTORY include/ DESTINATION include) -# Unit tests -if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) +# # Unit tests +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(moveit_resources REQUIRED) + find_package(tf2_geometry_msgs REQUIRED) + find_package(resource_retriever REQUIRED) + include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_robot_state test/robot_state_test.cpp) - target_link_libraries(test_robot_state moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + if(WIN32) + # set(append_library_dirs "$;$") + else() + set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils") + endif() + + ament_add_gtest(test_robot_state test/robot_state_test.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + + target_include_directories(test_robot_state PUBLIC + ${tf2_geometry_msgs_INCLUDE_DIRS} + ${urdf_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} + ${geometric_shapes_INCLUDE_DIRS} + ${srdfdom_INCLUDE_DIRS} + ) + + target_link_libraries(test_robot_state + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever + ) # As an executable, this benchmark is not run as a test by default - add_executable(robot_state_benchmark test/robot_state_benchmark.cpp) - target_link_libraries(robot_state_benchmark moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME} ${GTEST_LIBRARIES}) + ament_add_gtest(test_robot_state_benchmark test/robot_state_benchmark.cpp) + target_include_directories(test_robot_state_benchmark PUBLIC + ${tf2_geometry_msgs_INCLUDE_DIRS} + ${urdf_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} + ${geometric_shapes_INCLUDE_DIRS} + ${srdfdom_INCLUDE_DIRS} + ) + + target_link_libraries(test_robot_state_benchmark + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever + ) + + ament_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) + target_include_directories(test_robot_state_complex PUBLIC + ${tf2_geometry_msgs_INCLUDE_DIRS} + ${urdf_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} + ${geometric_shapes_INCLUDE_DIRS} + ${srdfdom_INCLUDE_DIRS} + ) + + target_link_libraries(test_robot_state_complex + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever + ) - catkin_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) - target_link_libraries(test_robot_state_complex moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + ament_add_gtest(test_aabb test/test_aabb.cpp) + target_include_directories(test_aabb PUBLIC + ${tf2_geometry_msgs_INCLUDE_DIRS} + ${urdf_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} + ${geometric_shapes_INCLUDE_DIRS} + ${srdfdom_INCLUDE_DIRS} + ) - catkin_add_gtest(test_aabb test/test_aabb.cpp) - target_link_libraries(test_aabb moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_aabb + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever + ) endif() From 3fc4471f9952019ecc7c1af11c9832cdbfdfef57 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 16 May 2019 12:52:22 +0200 Subject: [PATCH 05/13] robot_state - fixing wall timer Co-authored-by: anasarrak --- .../robot_state/include/moveit/robot_state/robot_state.h | 2 +- moveit_core/robot_state/src/robot_state.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index f94b2a3ff1..11ee4e0d89 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -952,7 +952,7 @@ as the new values that correspond to the group */ @param pose The pose the last link in the chain needs to achieve @param timeout The timeout passed to the kinematics solver on each attempt @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, double timeout = 0.0, + bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 106a13472b..37fd94f549 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1330,7 +1330,7 @@ bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eig return true; } -bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, double timeout, +bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg::Pose& pose, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { @@ -1815,7 +1815,8 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: // if no timeout has been specified, use the default one if (timeout < std::numeric_limits::epsilon()) timeout = jmg->getDefaultIKTimeout(); - ros::WallTime start = ros::WallTime::now(); + + auto start = std::chrono::system_clock::now(); double elapsed = 0; bool first_seed = true; @@ -1875,7 +1876,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: return true; } } - elapsed = (ros::WallTime::now() - start).toSec(); + elapsed = std::chrono::duration_cast(std::chrono::system_clock::now() - start).count(); first_seed = false; } while (elapsed < timeout); return false; From 47052c9f73cd0764395679bfb2efd5abcd9c96c0 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 16 May 2019 12:58:01 +0200 Subject: [PATCH 06/13] robot_state applying clang format --- .../include/moveit/robot_state/robot_state.h | 13 +++-- moveit_core/robot_state/src/conversions.cpp | 36 ++++++------ moveit_core/robot_state/src/robot_state.cpp | 58 +++++++++---------- 3 files changed, 54 insertions(+), 53 deletions(-) diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 11ee4e0d89..80b2e82666 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1060,8 +1060,8 @@ as the new values that correspond to the group */ * @param dt a time interval (seconds) * @param st a secondary task computation function */ - bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::msg::Twist& twist, const std::string& tip, double dt, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn()); + bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::msg::Twist& twist, const std::string& tip, + double dt, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn()); /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular group. @@ -1595,10 +1595,11 @@ as the new values that correspond to the group */ * from a planning_scene::PlanningScene), you will likely need to remove the * corresponding object from that world to avoid having collisions * detected against it. */ - void attachBody(const std::string& id, const std::vector& shapes, - const EigenSTL::vector_Isometry3d& attach_trans, const std::set& touch_links, - const std::string& link_name, - const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory()); + void + attachBody(const std::string& id, const std::vector& shapes, + const EigenSTL::vector_Isometry3d& attach_trans, const std::set& touch_links, + const std::string& link_name, + const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory()); /** @brief Add an attached body to a link * @param id The string id associated with the attached body diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 943c80b82a..75722da5f0 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -45,7 +45,6 @@ namespace moveit { namespace core { - // ******************************************** // * Internal (hidden) functions // ******************************************** @@ -60,7 +59,7 @@ static bool _jointStateToRobotState(const sensor_msgs::msg::JointState& joint_st if (joint_state.name.size() != joint_state.position.size()) { RCLCPP_ERROR(LOGGER, "Different number of names and positions in JointState message: %zu, %zu", - joint_state.name.size(), joint_state.position.size()); + joint_state.name.size(), joint_state.position.size()); return false; } @@ -105,8 +104,8 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::msg::MultiDOFJointSta if (error) RCLCPP_WARN(LOGGER, "The transform for multi-dof joints was specified in frame '%s' " - "but it was not possible to transform that to frame '%s'", - mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str()); + "but it was not possible to transform that to frame '%s'", + mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str()); } for (std::size_t i = 0; i < nj; ++i) @@ -221,7 +220,8 @@ static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::m } } -static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCollisionObject& aco, RobotState& state) +static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCollisionObject& aco, + RobotState& state) { if (aco.object.operation == moveit_msgs::msg::CollisionObject::ADD) { @@ -230,7 +230,7 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att if (aco.object.primitives.size() != aco.object.primitive_poses.size()) { RCLCPP_ERROR(LOGGER, "Number of primitive shapes does not match " - "number of poses in collision object message"); + "number of poses in collision object message"); return; } @@ -299,24 +299,25 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att { t0.setIdentity(); RCLCPP_ERROR(LOGGER, "Cannot properly transform from frame '%s'. " - "The pose of the attached body may be incorrect", - aco.object.header.frame_id.c_str()); + "The pose of the attached body may be incorrect", + aco.object.header.frame_id.c_str()); } Eigen::Isometry3d t = state.getGlobalLinkTransform(lm).inverse() * t0; for (std::size_t i = 0; i < poses.size(); ++i) poses[i] = t * poses[i]; } - if (shapes.empty()) { + if (shapes.empty()) + { RCLCPP_ERROR(LOGGER, "There is no geometry to attach to link '%s' as part of attached body '%s'", - aco.link_name.c_str(), aco.object.id.c_str()); + aco.link_name.c_str(), aco.object.id.c_str()); } else { if (state.clearAttachedBody(aco.object.id)) RCLCPP_DEBUG(LOGGER, "The robot state already had an object named '%s' attached to link '%s'. " - "The object was replaced.", - aco.object.id.c_str(), aco.link_name.c_str()); + "The object was replaced.", + aco.object.id.c_str(), aco.link_name.c_str()); state.attachBody(aco.object.id, shapes, poses, aco.touch_links, aco.link_name, aco.detach_posture); RCLCPP_DEBUG(LOGGER, "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str()); } @@ -329,7 +330,7 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att { if (!state.clearAttachedBody(aco.object.id)) RCLCPP_ERROR(LOGGER, "The attached body '%s' can not be removed because it does not exist", - aco.link_name.c_str()); + aco.link_name.c_str()); } else RCLCPP_ERROR(LOGGER, "Unknown collision object operation: %d", aco.object.operation); @@ -376,7 +377,8 @@ bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, Rob return result; } -bool robotStateMsgToRobotState(const moveit_msgs::msg::RobotState& robot_state, RobotState& state, bool copy_attached_bodies) +bool robotStateMsgToRobotState(const moveit_msgs::msg::RobotState& robot_state, RobotState& state, + bool copy_attached_bodies) { bool result = _robotStateMsgToRobotStateHelper(nullptr, robot_state, state, copy_attached_bodies); state.update(); @@ -391,7 +393,8 @@ bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::msg::Rob return result; } -void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::msg::RobotState& robot_state, bool copy_attached_bodies) +void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::msg::RobotState& robot_state, + bool copy_attached_bodies) { robot_state.is_diff = false; robotStateToJointStateMsg(state, robot_state.joint_state); @@ -535,8 +538,7 @@ void streamToRobotState(RobotState& state, const std::string& line, const std::s // Get a variable if (!std::getline(line_stream, cell, separator[0])) // ROS_ERROR_STREAM_NAMED(LOGNAME.c_str(), "Missing variable " << state.getVariableNames()[i]); - RCLCPP_ERROR(LOGGER, "Missing variable " , state.getVariableNames()[i].c_str()); - printf("show variablename %s\n", state.getVariableNames()[i].c_str()); + RCLCPP_ERROR(LOGGER, "Missing variable ", state.getVariableNames()[i].c_str()); state.getVariablePositions()[i] = boost::lexical_cast(cell.c_str()); } } diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 37fd94f549..a3d176facc 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1024,21 +1024,20 @@ const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& id) co if (jt == attached_body_map_.end()) { RCLCPP_ERROR(LOGGER, "Transform from frame '%s' to frame '%s' is not known " - "('%s' should be a link name or an attached body id).", - id.c_str(), robot_model_->getModelFrame().c_str(), id.c_str()); + "('%s' should be a link name or an attached body id).", + id.c_str(), robot_model_->getModelFrame().c_str(), id.c_str()); return IDENTITY_TRANSFORM; } const EigenSTL::vector_Isometry3d& tf = jt->second->getGlobalCollisionBodyTransforms(); if (tf.empty()) { - RCLCPP_ERROR(LOGGER, "Attached body '%s' has no geometry associated to it. No transform to return.", - id.c_str()); + RCLCPP_ERROR(LOGGER, "Attached body '%s' has no geometry associated to it. No transform to return.", id.c_str()); return IDENTITY_TRANSFORM; } if (tf.size() > 1) RCLCPP_DEBUG(LOGGER, "There are multiple geometries associated to attached body '%s'. " - "Returning the transform for the first one.", - id.c_str()); + "Returning the transform for the first one.", + id.c_str()); return tf[0]; } @@ -1053,8 +1052,8 @@ bool RobotState::knowsFrameTransform(const std::string& id) const } void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, - const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::Duration& dur, - bool include_attached) const + const std_msgs::msg::ColorRGBA& color, const std::string& ns, + const rclcpp::Duration& dur, bool include_attached) const { std::size_t cur_num = arr.markers.size(); getRobotMarkers(arr, link_names, include_attached); @@ -1162,7 +1161,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link if (!group->isLinkUpdated(link->getName())) { RCLCPP_ERROR(LOGGER, "Link name '%s' does not exist in the chain '%s' or is not a child for this chain", - link->getName().c_str(), group->getName().c_str()); + link->getName().c_str(), group->getName().c_str()); return false; } @@ -1262,8 +1261,8 @@ bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const Eigen::VectorXd return integrateVariableVelocity(jmg, qdot, dt, constraint); } -bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const geometry_msgs::msg::Twist& twist, const std::string& tip, - double dt, const GroupStateValidityCallbackFn& constraint) +bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const geometry_msgs::msg::Twist& twist, + const std::string& tip, double dt, const GroupStateValidityCallbackFn& constraint) { Eigen::Matrix t; tf2::fromMsg(twist, t); @@ -1469,8 +1468,8 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::string error_msg; if (!solver->supportsGroup(jmg, &error_msg)) { - RCLCPP_ERROR(LOGGER, "Kinematics solver %s does not support joint group %s. Error: %s", - typeid(*solver).name(), jmg->getName().c_str(), error_msg.c_str()); + RCLCPP_ERROR(LOGGER, "Kinematics solver %s does not support joint group %s. Error: %s", typeid(*solver).name(), + jmg->getName().c_str(), error_msg.c_str()); valid_solver = false; } } @@ -1495,8 +1494,8 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is if (consistency_limit_sets.size() > 1) { RCLCPP_ERROR(LOGGER, "Invalid number (%zu) of sets of consistency limits for a setFromIK request " - "that is being solved by a single IK solver", - consistency_limit_sets.size()); + "that is being solved by a single IK solver", + consistency_limit_sets.size()); return false; } else if (consistency_limit_sets.size() == 1) @@ -1552,7 +1551,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is if (ab_trans.size() != 1) { RCLCPP_ERROR(LOGGER, "Cannot use an attached body " - "with multiple geometries as a reference frame."); + "with multiple geometries as a reference frame."); return false; } pose_frame = ab->getAttachedLinkName(); @@ -1693,14 +1692,14 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: if (poses_in.size() != sub_groups.size()) { RCLCPP_ERROR(LOGGER, "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(), - sub_groups.size()); + sub_groups.size()); return false; } if (tips_in.size() != sub_groups.size()) { RCLCPP_ERROR(LOGGER, "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(), - sub_groups.size()); + sub_groups.size()); return false; } @@ -1715,7 +1714,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: if (consistency_limits[i].size() != sub_groups[i]->getVariableCount()) { RCLCPP_ERROR(LOGGER, "Number of joints in consistency_limits is %zu but it should be should be %u", i, - sub_groups[i]->getVariableCount()); + sub_groups[i]->getVariableCount()); return false; } } @@ -1787,8 +1786,8 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: if (pose_frame != solver_tip_frame) { - RCLCPP_ERROR(LOGGER, "Cannot compute IK for query pose reference frame '%s', desired: '%s'", - pose_frame.c_str(), solver_tip_frame.c_str()); + RCLCPP_ERROR(LOGGER, "Cannot compute IK for query pose reference frame '%s', desired: '%s'", pose_frame.c_str(), + solver_tip_frame.c_str()); return false; } } @@ -1927,10 +1926,9 @@ double RobotState::computeCartesianPath(const JointModelGroup* group, std::vecto if (max_step.translation <= 0.0 && max_step.rotation <= 0.0) { - RCLCPP_ERROR(LOGGER, - "Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and " - "MaxEEFStep.translation components must be non-negative and at least one component must be " - "greater than zero"); + RCLCPP_ERROR(LOGGER, "Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and " + "MaxEEFStep.translation components must be non-negative and at least one component must be " + "greater than zero"); return 0.0; } @@ -2060,8 +2058,8 @@ double RobotState::testRelativeJointSpaceJump(const JointModelGroup* group, std: if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH) { RCLCPP_WARN(LOGGER, "The computed trajectory is too short to detect jumps in joint-space " - "Need at least %zu steps, only got %zu. Try a lower max_step.", - MIN_STEPS_FOR_JUMP_THRESH, traj.size()); + "Need at least %zu steps, only got %zu. Try a lower max_step.", + MIN_STEPS_FOR_JUMP_THRESH, traj.size()); } std::vector dist_vector; @@ -2116,8 +2114,8 @@ double RobotState::testAbsoluteJointSpaceJump(const JointModelGroup* group, std: break; default: RCLCPP_WARN(LOGGER, "Joint %s has not supported type %s. \n" - "testAbsoluteJointSpaceJump only supports prismatic and revolute joints.", - joint->getName().c_str(), joint->getTypeName().c_str()); + "testAbsoluteJointSpaceJump only supports prismatic and revolute joints.", + joint->getName().c_str(), joint->getTypeName().c_str()); continue; } if (!data[type_index].check_) @@ -2127,7 +2125,7 @@ double RobotState::testAbsoluteJointSpaceJump(const JointModelGroup* group, std: if (distance > data[type_index].limit_) { RCLCPP_DEBUG(LOGGER, "Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s", distance, - data[type_index].limit_, joint->getName().c_str()); + data[type_index].limit_, joint->getName().c_str()); still_valid = false; break; } From 7b5dd7968023f03d267f73c87a6e02627bb18eb0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sat, 16 Mar 2019 23:55:33 +0100 Subject: [PATCH 07/13] robot_state, remove log.h --- .../include/moveit/robot_state/log.h | 53 ------------------- 1 file changed, 53 deletions(-) delete mode 100644 moveit_core/robot_state/include/moveit/robot_state/log.h diff --git a/moveit_core/robot_state/include/moveit/robot_state/log.h b/moveit_core/robot_state/include/moveit/robot_state/log.h deleted file mode 100644 index 99eabfd87d..0000000000 --- a/moveit_core/robot_state/include/moveit/robot_state/log.h +++ /dev/null @@ -1,53 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Acutronic Robotics AG - * 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 Willow Garage 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: Víctor Mayoral Vilches */ - -#ifndef MOVEIT_CORE_ROBOT_STATE_LOG_ -#define MOVEIT_CORE_ROBOT_STATE_LOG_ - -#include "rclcpp/rclcpp.hpp" - -namespace moveit -{ -namespace core -{ - -// Logger -rclcpp::Logger logger_robot_state = rclcpp::get_logger("robot_state"); - -}; -}; - -#endif From bab3a52b3f5637d5ef610574f25918dd1bf51477 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 17 May 2019 10:46:58 +0200 Subject: [PATCH 08/13] Update moveit_core/robot_state/CMakeLists.txt Co-Authored-By: Dave Coleman --- moveit_core/robot_state/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index f63131f910..e0f7225420 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -13,7 +13,8 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${urdfdom_LIBRARIES} ${geometric_shapes_LIBRARIES} ${urdfdom_headers_LIBRARIES} - ${Boost_LIBRARIES}) + ${Boost_LIBRARIES} +) ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_robot_model moveit_kinematics_base From 4d3c4cbba76f7c122a7d0878d04054f01188f551 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 17 May 2019 10:47:12 +0200 Subject: [PATCH 09/13] Update moveit_core/robot_state/CMakeLists.txt Co-Authored-By: Dave Coleman --- moveit_core/robot_state/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index e0f7225420..6627950a72 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -28,7 +28,7 @@ install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION lib) install(DIRECTORY include/ DESTINATION include) -# # Unit tests +# Unit tests if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(moveit_resources REQUIRED) From 3dde080e95798f69022d5f5112b8ffcacf2554cc Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 17 May 2019 10:57:54 +0200 Subject: [PATCH 10/13] robot_state restoring clang warnings compiler option --- moveit_core/robot_state/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 6627950a72..095496a4de 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -1,5 +1,10 @@ set(MOVEIT_LIB_NAME moveit_robot_state) +if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + # clang is picky about typeid(*solver) + add_compile_options(-Wno-potentially-evaluated-expression) +endif() + add_library(${MOVEIT_LIB_NAME} SHARED src/attached_body.cpp src/conversions.cpp From f1f8f0861ffbc5aa10996b04fb0c165d66bb26ac Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 17 May 2019 11:02:53 +0200 Subject: [PATCH 11/13] robot_state fixing indentation CMakelists.txt --- moveit_core/robot_state/CMakeLists.txt | 110 ++++++++++++------------- 1 file changed, 55 insertions(+), 55 deletions(-) diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 095496a4de..3f3008188e 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -35,21 +35,21 @@ install(DIRECTORY include/ DESTINATION include) # Unit tests if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gtest REQUIRED) find_package(moveit_resources REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(resource_retriever REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) + include_directories(${moveit_resources_INCLUDE_DIRS}) if(WIN32) - # set(append_library_dirs "$;$") + # set(append_library_dirs "$;$") else() - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils") + set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils") endif() - ament_add_gtest(test_robot_state test/robot_state_test.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") + ament_add_gtest(test_robot_state test/robot_state_test.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") target_include_directories(test_robot_state PUBLIC ${tf2_geometry_msgs_INCLUDE_DIRS} @@ -60,22 +60,22 @@ if(BUILD_TESTING) ) target_link_libraries(test_robot_state - moveit_test_utils - moveit_utils - moveit_exceptions - ${tf2_geometry_msgs_LIBRARIES} - ${srdfdom_LIBRARIES} - ${MOVEIT_LIB_NAME} - ${moveit_profiler_LIBRARIES} - ${urdf_LIBRARIES} - ${visualization_msgs_LIBRARIES} - ${geometric_shapes_LIBRARIES} - resource_retriever::resource_retriever + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever ) - # As an executable, this benchmark is not run as a test by default - ament_add_gtest(test_robot_state_benchmark test/robot_state_benchmark.cpp) - target_include_directories(test_robot_state_benchmark PUBLIC + # As an executable, this benchmark is not run as a test by default + ament_add_gtest(test_robot_state_benchmark test/robot_state_benchmark.cpp) + target_include_directories(test_robot_state_benchmark PUBLIC ${tf2_geometry_msgs_INCLUDE_DIRS} ${urdf_INCLUDE_DIRS} ${visualization_msgs_INCLUDE_DIRS} @@ -84,20 +84,20 @@ if(BUILD_TESTING) ) target_link_libraries(test_robot_state_benchmark - moveit_test_utils - moveit_utils - moveit_exceptions - ${tf2_geometry_msgs_LIBRARIES} - ${srdfdom_LIBRARIES} - ${MOVEIT_LIB_NAME} - ${moveit_profiler_LIBRARIES} - ${urdf_LIBRARIES} - ${visualization_msgs_LIBRARIES} - ${geometric_shapes_LIBRARIES} - resource_retriever::resource_retriever + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever ) - ament_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) + ament_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) target_include_directories(test_robot_state_complex PUBLIC ${tf2_geometry_msgs_INCLUDE_DIRS} ${urdf_INCLUDE_DIRS} @@ -107,20 +107,20 @@ if(BUILD_TESTING) ) target_link_libraries(test_robot_state_complex - moveit_test_utils - moveit_utils - moveit_exceptions - ${tf2_geometry_msgs_LIBRARIES} - ${srdfdom_LIBRARIES} - ${MOVEIT_LIB_NAME} - ${moveit_profiler_LIBRARIES} - ${urdf_LIBRARIES} - ${visualization_msgs_LIBRARIES} - ${geometric_shapes_LIBRARIES} - resource_retriever::resource_retriever + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever ) - ament_add_gtest(test_aabb test/test_aabb.cpp) + ament_add_gtest(test_aabb test/test_aabb.cpp) target_include_directories(test_aabb PUBLIC ${tf2_geometry_msgs_INCLUDE_DIRS} ${urdf_INCLUDE_DIRS} @@ -130,16 +130,16 @@ if(BUILD_TESTING) ) target_link_libraries(test_aabb - moveit_test_utils - moveit_utils - moveit_exceptions - ${tf2_geometry_msgs_LIBRARIES} - ${srdfdom_LIBRARIES} - ${MOVEIT_LIB_NAME} - ${moveit_profiler_LIBRARIES} - ${urdf_LIBRARIES} - ${visualization_msgs_LIBRARIES} - ${geometric_shapes_LIBRARIES} - resource_retriever::resource_retriever + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever ) endif() From 0525925b7f54fbf3661464861537bcbbbe56bd7f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 20 May 2019 16:21:44 +0200 Subject: [PATCH 12/13] Update moveit_core/robot_state/src/conversions.cpp Co-Authored-By: Henning Kayser --- moveit_core/robot_state/src/conversions.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 75722da5f0..006f559153 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -537,7 +537,6 @@ void streamToRobotState(RobotState& state, const std::string& line, const std::s { // Get a variable if (!std::getline(line_stream, cell, separator[0])) - // ROS_ERROR_STREAM_NAMED(LOGNAME.c_str(), "Missing variable " << state.getVariableNames()[i]); RCLCPP_ERROR(LOGGER, "Missing variable ", state.getVariableNames()[i].c_str()); state.getVariablePositions()[i] = boost::lexical_cast(cell.c_str()); } From a5a795f51aeb1c5444fff59b75659067b744073e Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Tue, 21 May 2019 14:49:10 -0600 Subject: [PATCH 13/13] tabs to spaces --- moveit_core/robot_state/CMakeLists.txt | 192 ++++++++++++------------- 1 file changed, 96 insertions(+), 96 deletions(-) diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 3f3008188e..cfe45f04bb 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -35,111 +35,111 @@ install(DIRECTORY include/ DESTINATION include) # Unit tests if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(moveit_resources REQUIRED) - find_package(tf2_geometry_msgs REQUIRED) - find_package(resource_retriever REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(moveit_resources REQUIRED) + find_package(tf2_geometry_msgs REQUIRED) + find_package(resource_retriever REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) + include_directories(${moveit_resources_INCLUDE_DIRS}) - if(WIN32) - # set(append_library_dirs "$;$") - else() - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils") - endif() + if(WIN32) + # set(append_library_dirs "$;$") + else() + set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils") + endif() - ament_add_gtest(test_robot_state test/robot_state_test.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") + ament_add_gtest(test_robot_state test/robot_state_test.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") - target_include_directories(test_robot_state PUBLIC - ${tf2_geometry_msgs_INCLUDE_DIRS} - ${urdf_INCLUDE_DIRS} - ${visualization_msgs_INCLUDE_DIRS} - ${geometric_shapes_INCLUDE_DIRS} - ${srdfdom_INCLUDE_DIRS} - ) + target_include_directories(test_robot_state PUBLIC + ${tf2_geometry_msgs_INCLUDE_DIRS} + ${urdf_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} + ${geometric_shapes_INCLUDE_DIRS} + ${srdfdom_INCLUDE_DIRS} + ) - target_link_libraries(test_robot_state - moveit_test_utils - moveit_utils - moveit_exceptions - ${tf2_geometry_msgs_LIBRARIES} - ${srdfdom_LIBRARIES} - ${MOVEIT_LIB_NAME} - ${moveit_profiler_LIBRARIES} - ${urdf_LIBRARIES} - ${visualization_msgs_LIBRARIES} - ${geometric_shapes_LIBRARIES} - resource_retriever::resource_retriever - ) + target_link_libraries(test_robot_state + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever + ) - # As an executable, this benchmark is not run as a test by default - ament_add_gtest(test_robot_state_benchmark test/robot_state_benchmark.cpp) - target_include_directories(test_robot_state_benchmark PUBLIC - ${tf2_geometry_msgs_INCLUDE_DIRS} - ${urdf_INCLUDE_DIRS} - ${visualization_msgs_INCLUDE_DIRS} - ${geometric_shapes_INCLUDE_DIRS} - ${srdfdom_INCLUDE_DIRS} - ) + # As an executable, this benchmark is not run as a test by default + ament_add_gtest(test_robot_state_benchmark test/robot_state_benchmark.cpp) + target_include_directories(test_robot_state_benchmark PUBLIC + ${tf2_geometry_msgs_INCLUDE_DIRS} + ${urdf_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} + ${geometric_shapes_INCLUDE_DIRS} + ${srdfdom_INCLUDE_DIRS} + ) - target_link_libraries(test_robot_state_benchmark - moveit_test_utils - moveit_utils - moveit_exceptions - ${tf2_geometry_msgs_LIBRARIES} - ${srdfdom_LIBRARIES} - ${MOVEIT_LIB_NAME} - ${moveit_profiler_LIBRARIES} - ${urdf_LIBRARIES} - ${visualization_msgs_LIBRARIES} - ${geometric_shapes_LIBRARIES} - resource_retriever::resource_retriever - ) + target_link_libraries(test_robot_state_benchmark + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever + ) - ament_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) - target_include_directories(test_robot_state_complex PUBLIC - ${tf2_geometry_msgs_INCLUDE_DIRS} - ${urdf_INCLUDE_DIRS} - ${visualization_msgs_INCLUDE_DIRS} - ${geometric_shapes_INCLUDE_DIRS} - ${srdfdom_INCLUDE_DIRS} - ) + ament_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) + target_include_directories(test_robot_state_complex PUBLIC + ${tf2_geometry_msgs_INCLUDE_DIRS} + ${urdf_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} + ${geometric_shapes_INCLUDE_DIRS} + ${srdfdom_INCLUDE_DIRS} + ) - target_link_libraries(test_robot_state_complex - moveit_test_utils - moveit_utils - moveit_exceptions - ${tf2_geometry_msgs_LIBRARIES} - ${srdfdom_LIBRARIES} - ${MOVEIT_LIB_NAME} - ${moveit_profiler_LIBRARIES} - ${urdf_LIBRARIES} - ${visualization_msgs_LIBRARIES} - ${geometric_shapes_LIBRARIES} - resource_retriever::resource_retriever - ) + target_link_libraries(test_robot_state_complex + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever + ) - ament_add_gtest(test_aabb test/test_aabb.cpp) - target_include_directories(test_aabb PUBLIC - ${tf2_geometry_msgs_INCLUDE_DIRS} - ${urdf_INCLUDE_DIRS} - ${visualization_msgs_INCLUDE_DIRS} - ${geometric_shapes_INCLUDE_DIRS} - ${srdfdom_INCLUDE_DIRS} - ) + ament_add_gtest(test_aabb test/test_aabb.cpp) + target_include_directories(test_aabb PUBLIC + ${tf2_geometry_msgs_INCLUDE_DIRS} + ${urdf_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} + ${geometric_shapes_INCLUDE_DIRS} + ${srdfdom_INCLUDE_DIRS} + ) - target_link_libraries(test_aabb - moveit_test_utils - moveit_utils - moveit_exceptions - ${tf2_geometry_msgs_LIBRARIES} - ${srdfdom_LIBRARIES} - ${MOVEIT_LIB_NAME} - ${moveit_profiler_LIBRARIES} - ${urdf_LIBRARIES} - ${visualization_msgs_LIBRARIES} - ${geometric_shapes_LIBRARIES} - resource_retriever::resource_retriever - ) + target_link_libraries(test_aabb + moveit_test_utils + moveit_utils + moveit_exceptions + ${tf2_geometry_msgs_LIBRARIES} + ${srdfdom_LIBRARIES} + ${MOVEIT_LIB_NAME} + ${moveit_profiler_LIBRARIES} + ${urdf_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever + ) endif()