diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 4f53cc428f..17ee01f149 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -1,41 +1,64 @@ 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}") - -target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model moveit_kinematics_base moveit_transforms ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) - -add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) +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}) +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 ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) - -# Unit tests -if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources 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 ${catkin_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}) - - 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}) + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) +install(DIRECTORY include/ DESTINATION include) - 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}) -endif() +# # Unit tests +# if(BUILD_TESTING) +# find_package(ament_cmake_gtest REQUIRED) +# find_package(moveit_resources REQUIRED) +# +# include_directories(${moveit_resources_INCLUDE_DIRS}) +# +# ament_add_gtest(test_robot_state test/robot_state_test.cpp) +# ament_target_dependencies(test_robot_state +# tf2_geometry_msgs +# ${MOVEIT_LIB_NAME} +# urdf +# visualization_msgs +# ) +# +# # As an executable, this benchmark is not run as a test by default +# add_executable(robot_state_benchmark test/robot_state_benchmark.cpp) +# ament_target_dependencies(robot_state_benchmark +# moveit_test_utils +# tf2_geometry_msgs +# ${MOVEIT_LIB_NAME} +# urdf +# visualization_msgs +# ) +# +# ament_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) +# ament_target_dependencies(test_robot_state_complex +# moveit_test_utils +# ${MOVEIT_LIB_NAME} +# urdf +# visualization_msgs +# ) +# +# ament_add_gtest(test_aabb test/test_aabb.cpp) +# ament_target_dependencies(test_aabb +# moveit_test_utils +# ${MOVEIT_LIB_NAME} +# urdf +# visualization_msgs +# tf2_geometry_msgs +# ) +# 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..963015eb6a 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 @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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: Ioan Sucan */ @@ -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,12 +147,12 @@ 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_; }; -} -} +} // namespace core +} // namespace moveit #endif 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 8f19031dbf..2b308be148 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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 the 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: Ioan Sucan, Dave Coleman */ @@ -39,8 +39,8 @@ #include #include -#include -#include +#include +#include namespace moveit { @@ -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); /** @@ -141,7 +141,7 @@ void robotStateToStream(const RobotState& state, std::ostream& out, * \return true on success */ void streamToRobotState(RobotState& state, const std::string& line, const std::string& separator = ","); -} -} +} // namespace core +} // namespace moveit #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 b0ba9b05d3..ee7e83200b 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 @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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: Ioan Sucan */ @@ -41,10 +41,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -516,9 +516,6 @@ class RobotState return effort_[index]; } - /** \brief Invert velocity if present. */ - void invertVelocity(); - /** @} */ /** \name Getting and setting joint positions, velocities, accelerations and effort @@ -950,71 +947,48 @@ as the new values that correspond to the group */ set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. @param pose The pose the last link in the chain needs to achieve + @param attempts The number of times IK is attempted @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, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, unsigned int attempts = 0, + 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. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. @param pose The pose the \e tip link in the chain needs to achieve @param tip The name of the link the pose is specified for + @param attempts The number of times IK is attempted @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, - double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, const std::string& tip, + unsigned int attempts = 0, 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. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. @param pose The pose the last link in the chain needs to achieve @param tip The name of the link the pose is specified for + @param attempts The number of times IK is attempted @param timeout The timeout passed to the kinematics solver on each attempt */ - bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int attempts = 0, + 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. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. @param pose The pose the last link in the chain needs to achieve + @param attempts The number of times IK is attempted @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 Eigen::Isometry3d& pose, const std::string& tip, - double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + unsigned int attempts = 0, 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. @@ -1022,42 +996,28 @@ as the new values that correspond to the group */ @param pose The pose the last link in the chain needs to achieve @param tip The name of the frame for which IK is attempted. @param consistency_limits This specifies the desired distance between the solution and the seed state + @param attempts The number of times IK is attempted @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 Eigen::Isometry3d& pose, const std::string& tip, - const std::vector& consistency_limits, double timeout = 0.0, + const std::vector& consistency_limits, unsigned int attempts = 0, 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. + /** \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 is available for each sub-group, then the joint values can be set by computing inverse kinematics. The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed to be in the same order as the order of the sub-groups in this group. Returns true on success. @param poses The poses the last link in each chain needs to achieve @param tips The names of the frames for which IK is attempted. + @param attempts The number of times IK is attempted @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 EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, double timeout = 0.0, + const std::vector& tips, unsigned int attempts = 0, 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 @@ -1067,21 +1027,14 @@ as the new values that correspond to the group */ @param poses The poses the last link in each chain needs to achieve @param tips The names of the frames for which IK is attempted. @param consistency_limits This specifies the desired distance between the solution and the seed state + @param attempts The number of times IK is attempted @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 EigenSTL::vector_Isometry3d& poses, const std::vector& tips, const std::vector >& consistency_limits, - double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + unsigned int attempts = 0, 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 @@ -1089,22 +1042,15 @@ as the new values that correspond to the group */ @param poses The poses the last link in each chain needs to achieve @param tips The names of the frames for which IK is attempted. @param consistency_limits This specifies the desired distance between the solution and the seed state + @param attempts The number of times IK is attempted @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 setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, const std::vector& tips, - const std::vector >& consistency_limits, double timeout = 0.0, + const std::vector >& consistency_limits, unsigned int attempts = 0, + 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,8 +1069,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::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. @@ -1340,7 +1286,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); @@ -1565,17 +1511,6 @@ as the new values that correspond to the group */ updateMimicJoint(joint); } } - - /// Call harmonizePosition() for all joints / all joints in group / given joint - void harmonizePositions(); - void harmonizePositions(const JointModelGroup* joint_group); - void harmonizePosition(const JointModel* joint) - { - if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex())) - // no need to mark transforms dirty, as the transform hasn't changed - updateMimicJoint(joint); - } - void enforceVelocityBounds(const JointModel* joint) { joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex()); @@ -1658,10 +1593,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::JointTrajectory& detach_posture = trajectory_msgs::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 @@ -1680,7 +1616,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 +1687,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 +1698,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 +1712,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(); @@ -1792,6 +1728,10 @@ as the new values that correspond to the group */ void printStatePositions(std::ostream& out = std::cout) const; + /** \brief Output to console the current state of the robot's joint limits */ + void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, + std::ostream& out = std::cout) const; + void printStateInfo(std::ostream& out = std::cout) const; void printTransforms(std::ostream& out = std::cout) const; @@ -1918,7 +1858,9 @@ as the new values that correspond to the group */ /** \brief Operator overload for printing variable bounds to a stream */ std::ostream& operator<<(std::ostream& out, const RobotState& s); -} -} + +rclcpp::Clock clock_ros; +} // namespace core +} // namespace moveit #endif diff --git a/moveit_core/robot_state/src/attached_body.cpp b/moveit_core/robot_state/src/attached_body.cpp index 97722c99f9..9e95711e6e 100644 --- a/moveit_core/robot_state/src/attached_body.cpp +++ b/moveit_core/robot_state/src/attached_body.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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: Ioan Sucan */ @@ -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 e751feb311..006b581092 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2011-2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2011-2013, Willow Garage, Inc. + * 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 the 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: Ioan Sucan, Dave Coleman */ @@ -39,25 +39,27 @@ #include #include #include +#include "rclcpp/rclcpp.hpp" namespace moveit { namespace core { -const std::string LOGNAME = "robot_state"; - // ******************************************** // * Internal (hidden) functions // ******************************************** namespace { -static bool _jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state) +// Logger +rclcpp::Logger LOGGER = rclcpp::get_logger("robot_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", - 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()); return false; } @@ -66,13 +68,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, "Different number of names, values or frames in MultiDOFJointState message."); return false; } @@ -94,16 +96,17 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::MultiDOFJointState& m } catch (std::exception& ex) { - ROS_ERROR_NAMED(LOGNAME, "Caught %s", ex.what()); + RCLCPP_ERROR(LOGGER, "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' " - "but it was not possible to transform that to frame '%s'", - mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str()); + 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()); } for (std::size_t i = 0; i < nj; ++i) @@ -111,10 +114,12 @@ 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, "No joint matching multi-dof joint '%s'", joint_name.c_str()); error = true; continue; } + // if frames do not mach, attempt to transform + Eigen::Isometry3d transf = tf2::transformToEigen(mjs.transforms[i]); // if frames do not mach, attempt to transform if (use_inv_t) @@ -126,14 +131,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 +161,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 +187,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,14 +216,15 @@ 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); } } } -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) { @@ -226,20 +232,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 " - "number of poses in collision object message"); + 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()) { - ROS_ERROR_NAMED(LOGNAME, "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()) { - ROS_ERROR_NAMED(LOGNAME, "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,9 +301,10 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att else { t0.setIdentity(); - ROS_ERROR_NAMED(LOGNAME, "Cannot properly transform from frame '%s'. " - "The pose of the attached body may be incorrect", - aco.object.header.frame_id.c_str()); + 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()); } Eigen::Isometry3d t = state.getGlobalLinkTransform(lm).inverse() * t0; for (std::size_t i = 0; i < poses.size(); ++i) @@ -305,30 +312,33 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att } 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()); + { + 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)) - ROS_DEBUG_NAMED(LOGNAME, "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()); + 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); - ROS_DEBUG_NAMED(LOGNAME, "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 - ROS_ERROR_NAMED(LOGNAME, "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)) - ROS_ERROR_NAMED(LOGNAME, "The attached body '%s' can not be removed because it does not exist", - aco.link_name.c_str()); + RCLCPP_ERROR(LOGGER, "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, "Unknown collision object operation: %d", aco.object.operation); } static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::msg::RobotState& robot_state, @@ -339,7 +349,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, "Found empty JointState message"); return false; } @@ -357,7 +367,7 @@ static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_ return valid; } -} +} // namespace // ******************************************** @@ -365,14 +375,15 @@ 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(); 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(); @@ -387,7 +398,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); @@ -410,10 +422,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 +442,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, "Invalid point_id"); return false; } if (trajectory.joint_names.empty()) { - ROS_ERROR_NAMED(LOGNAME, "No joint names specified"); + RCLCPP_ERROR(LOGGER, "No joint names specified"); return false; } @@ -530,8 +542,9 @@ 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]); - + // 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 b7ff500b4b..242315843c 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ @@ -41,21 +41,24 @@ #include #include #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 * 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) @@ -140,9 +143,10 @@ void RobotState::copyFrom(const RobotState& other) if (dirty_link_transforms_ == robot_model_->getRootJoint()) { // everything is dirty; no point in copying transforms; copy positions, potentially velocity & acceleration - memcpy(position_, other.position_, robot_model_->getVariableCount() * sizeof(double) * - (1 + ((has_velocity_ || has_acceleration_ || has_effort_) ? 1 : 0) + - ((has_acceleration_ || has_effort_) ? 1 : 0))); + memcpy(position_, other.position_, + robot_model_->getVariableCount() * sizeof(double) * + (1 + ((has_velocity_ || has_acceleration_ || has_effort_) ? 1 : 0) + + ((has_acceleration_ || has_effort_) ? 1 : 0))); // and just initialize transforms initTransforms(); } @@ -173,7 +177,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, "Returning dirty joint transforms for joint '%s'", joint->getName().c_str()); return false; } return true; @@ -183,7 +187,7 @@ bool RobotState::checkLinkTransforms() const { if (dirtyLinkTransforms()) { - ROS_WARN_NAMED(LOGNAME, "Returning dirty link transforms"); + RCLCPP_WARN(LOGGER, "Returning dirty link transforms"); return false; } return true; @@ -193,7 +197,7 @@ bool RobotState::checkCollisionTransforms() const { if (dirtyCollisionBodyTransforms()) { - ROS_WARN_NAMED(LOGNAME, "Returning dirty collision body transforms"); + RCLCPP_WARN(LOGGER, "Returning dirty collision body transforms"); return false; } return true; @@ -429,20 +433,11 @@ void RobotState::setVariableEffort(const std::vector& variable_name effort_[robot_model_->getVariableIndex(variable_names[i])] = variable_effort[i]; } -void RobotState::invertVelocity() -{ - if (has_velocity_) - { - for (size_t i = 0; i < robot_model_->getVariableCount(); ++i) - velocity_[i] *= -1; - } -} - 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, "Unable to set joint efforts because array is being used for accelerations"); return; } has_effort_ = true; @@ -745,18 +740,6 @@ void RobotState::enforceBounds(const JointModelGroup* joint_group) enforceBounds(jm[i]); } -void RobotState::harmonizePositions() -{ - for (const JointModel* jm : robot_model_->getActiveJointModels()) - harmonizePosition(jm); -} - -void RobotState::harmonizePositions(const JointModelGroup* joint_group) -{ - for (const JointModel* jm : joint_group->getActiveJointModels()) - harmonizePosition(jm); -} - std::pair RobotState::getMinDistanceToPositionBounds() const { return getMinDistanceToPositionBounds(robot_model_->getActiveJointModels()); @@ -872,7 +855,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, "Attached body '%s' not found", id.c_str()); return nullptr; } else @@ -889,7 +872,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); @@ -1013,22 +996,23 @@ 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 " - "('%s' should be a link name or an attached body id).", - id.c_str(), robot_model_->getModelFrame().c_str(), id.c_str()); + 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; } 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.", - 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) - ROS_DEBUG_NAMED(LOGNAME, "There are multiple geometries associated to attached body '%s'. " - "Returning the transform for the first one.", - id.c_str()); + 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]; } @@ -1042,9 +1026,9 @@ 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, - bool include_attached) 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 { std::size_t cur_num = arr.markers.size(); getRobotMarkers(arr, link_names, include_attached); @@ -1058,13 +1042,16 @@ 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(); + // ros::Time tm = ros::Time::now(); + + builtin_interfaces::msg::Time tm = clock_ros.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, "Trying to get marker for link '%s'", link_names[i].c_str()); const LinkModel* lm = robot_model_->getLinkModel(link_names[i]); if (!lm) continue; @@ -1075,7 +1062,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)) @@ -1094,7 +1081,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; @@ -1144,14 +1131,14 @@ 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, "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", - link->getName().c_str(), group->getName().c_str()); + 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; } @@ -1167,7 +1154,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, "Point from reference origin expressed in world coordinates: %f %f %f", point_transform.x(), point_transform.y(), point_transform.z()); @@ -1179,11 +1166,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, "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, "Joint: %s",link_state->getParentJointState()->getName().c_str()); */ const JointModel* pjm = link->getParentJointModel(); if (pjm->getVariableCount() > 0) @@ -1221,7 +1208,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, "Unknown type of joint in Jacobian computation"); } if (pjm == root_joint_model) break; @@ -1251,8 +1238,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::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); @@ -1319,55 +1306,55 @@ bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eig return true; } -bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, double timeout, - const GroupStateValidityCallbackFn& constraint, +bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg::Pose& pose, unsigned int attempts, + double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { 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, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } - return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint, options); + return setFromIK(jmg, pose, solver->getTipFrame(), attempts, timeout, constraint, options); } -bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, const std::string& tip, - double timeout, const GroupStateValidityCallbackFn& constraint, +bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg::Pose& pose, const std::string& tip, + unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { Eigen::Isometry3d mat; tf2::fromMsg(pose, mat); static std::vector consistency_limits; - return setFromIK(jmg, mat, tip, consistency_limits, timeout, constraint, options); + return setFromIK(jmg, mat, tip, consistency_limits, attempts, timeout, constraint, options); } -bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose, double timeout, - const GroupStateValidityCallbackFn& constraint, +bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose, unsigned int attempts, + double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { 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, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } static std::vector consistency_limits; - return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, timeout, constraint, options); + return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, attempts, timeout, constraint, options); } bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose_in, const std::string& tip_in, - double timeout, const GroupStateValidityCallbackFn& constraint, + unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { static std::vector consistency_limits; - return setFromIK(jmg, pose_in, tip_in, consistency_limits, timeout, constraint, options); + return setFromIK(jmg, pose_in, tip_in, consistency_limits, attempts, timeout, constraint, options); } 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(); @@ -1380,7 +1367,7 @@ bool ikCallbackFnAdapter(RobotState* state, const JointModelGroup* group, error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; return true; } -} +} // namespace bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver) { @@ -1395,7 +1382,8 @@ 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."); + // ROS_ERROR_STREAM_NAMED(LOGNAME, "IK frame '" << ik_frame << "' does not exist."); + RCLCPP_ERROR(LOGGER, "The following IK frame does not exist:", ik_frame.c_str()); return false; } pose = getGlobalLinkTransform(lm).inverse() * pose; @@ -1404,7 +1392,7 @@ bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& } bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose_in, const std::string& tip_in, - const std::vector& consistency_limits_in, double timeout, + const std::vector& consistency_limits_in, unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { @@ -1418,28 +1406,28 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& std::vector > consistency_limits; consistency_limits.push_back(consistency_limits_in); - return setFromIK(jmg, poses, tips, consistency_limits, timeout, constraint, options); + return setFromIK(jmg, poses, tips, consistency_limits, attempts, timeout, constraint, options); } bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in, - const std::vector& tips_in, double timeout, + const std::vector& tips_in, unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { - const std::vector > consistency_limits; - return setFromIK(jmg, poses_in, tips_in, consistency_limits, timeout, constraint, options); + static const std::vector > CONSISTENCY_LIMITS; + return setFromIK(jmg, poses_in, tips_in, CONSISTENCY_LIMITS, attempts, timeout, constraint, options); } bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in, const std::vector& tips_in, - const std::vector >& consistency_limit_sets, double timeout, - const GroupStateValidityCallbackFn& constraint, + const std::vector >& consistency_limit_sets, unsigned int attempts, + double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { // 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, "Number of poses must be the same as number of tips"); return false; } @@ -1458,8 +1446,8 @@ 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", - 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; } } @@ -1470,11 +1458,11 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is if (poses_in.size() > 1) { // Forward to setFromIKSubgroups() to allow different subgroup IK solvers to work together - return setFromIKSubgroups(jmg, poses_in, tips_in, consistency_limit_sets, timeout, constraint, options); + return setFromIKSubgroups(jmg, poses_in, tips_in, consistency_limit_sets, attempts, timeout, constraint, options); } else { - ROS_ERROR_NAMED(LOGNAME, "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; } } @@ -1483,9 +1471,10 @@ 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 " - "that is being solved by a single IK solver", - consistency_limit_sets.size()); + 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; } else if (consistency_limit_sets.size() == 1) @@ -1497,7 +1486,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) @@ -1540,8 +1529,8 @@ 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 " - "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(); @@ -1552,7 +1541,8 @@ 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."); + // ROS_ERROR_STREAM_NAMED(LOGNAME, "Pose frame '" << pose_frame << "' does not exist."); + RCLCPP_ERROR(LOGGER, "The following Pose Frame does not exist ", pose_frame.c_str()); return false; } const robot_model::LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms(); @@ -1579,12 +1569,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, "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, "Available tip frames: [%s]", ss.str().c_str()); return false; } @@ -1592,7 +1582,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 @@ -1622,7 +1612,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 @@ -1636,6 +1626,9 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is if (timeout < std::numeric_limits::epsilon()) timeout = jmg->getDefaultIKTimeout(); + if (attempts == 0) + attempts = jmg->getDefaultIKAttempts(); + // set callback function kinematics::KinematicsBase::IKCallbackFn ik_callback_fn; if (constraint) @@ -1644,32 +1637,66 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // Bijection const std::vector& bij = jmg->getKinematicsSolverJointBijection(); + bool first_seed = true; std::vector initial_values; - copyJointGroupPositions(jmg, initial_values); - std::vector seed(bij.size()); - for (std::size_t i = 0; i < bij.size(); ++i) - seed[i] = initial_values[bij[i]]; + for (unsigned int st = 0; st < attempts; ++st) + { + std::vector seed(bij.size()); - // compute the IK solution - std::vector ik_sol; - moveit_msgs::msg::MoveItErrorCodes error; + // the first seed is the current robot state joint values + // // compute the IK solution, melodic-sync + // std::vector ik_sol; + // moveit_msgs::msg::MoveItErrorCodes error; - if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, error, options, - this)) - { - std::vector solution(bij.size()); - for (std::size_t i = 0; i < bij.size(); ++i) - solution[bij[i]] = ik_sol[i]; - setJointGroupPositions(jmg, solution); - return true; + if (first_seed) + { + first_seed = false; + copyJointGroupPositions(jmg, initial_values); + for (std::size_t i = 0; i < bij.size(); ++i) + seed[i] = initial_values[bij[i]]; + } + else + { + RCLCPP_DEBUG(LOGGER, "Rerunning IK solver with random joint positions"); + + // sample a random seed + random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator(); + std::vector random_values; + jmg->getVariableRandomPositions(rng, random_values); + for (std::size_t i = 0; i < bij.size(); ++i) + seed[i] = random_values[bij[i]]; + + if (options.lock_redundant_joints) + { + std::vector red_joints; + solver->getRedundantJoints(red_joints); + copyJointGroupPositions(jmg, initial_values); + for (std::size_t i = 0; i < red_joints.size(); ++i) + seed[red_joints[i]] = initial_values[bij[red_joints[i]]]; + } + } + + // compute the IK solution + std::vector ik_sol; + moveit_msgs::msg::MoveItErrorCodes error; + + if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, error, options, + this)) + { + std::vector solution(bij.size()); + for (std::size_t i = 0; i < bij.size(); ++i) + solution[bij[i]] = ik_sol[i]; + setJointGroupPositions(jmg, solution); + return true; + } } return false; } bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in, const std::vector& tips_in, - const std::vector >& consistency_limits, double timeout, - const GroupStateValidityCallbackFn& constraint, + const std::vector >& consistency_limits, unsigned int attempts, + double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { // Assume we have already ran setFromIK() and those checks @@ -1681,21 +1708,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(), - 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()); 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(), - 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()); 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, "Number of consistency limit vectors must be the same as number of sub-groups"); return false; } @@ -1703,8 +1730,8 @@ 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, - 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()); return false; } } @@ -1716,7 +1743,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, "Could not find solver for group '%s'", sub_groups[i]->getName().c_str()); return false; } solvers.push_back(solver); @@ -1752,7 +1779,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, "Cannot use an attached body with multiple geometries as a reference frame."); return false; } pose_frame = ab->getAttachedLinkName(); @@ -1776,14 +1803,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'", - 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; } } // 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); @@ -1801,18 +1828,16 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: ik_queries[i].orientation.w = quat.w(); } + if (attempts == 0) + attempts = jmg->getDefaultIKAttempts(); + // 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(); - double elapsed = 0; bool first_seed = true; - unsigned int attempts = 0; - do + for (unsigned int st = 0; st < attempts; ++st) { - ++attempts; - ROS_DEBUG_NAMED(LOGNAME, "IK attempt: %d", attempts); bool found_solution = true; for (std::size_t sg = 0; sg < sub_groups.size(); ++sg) { @@ -1840,8 +1865,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: std::vector ik_sol; moveit_msgs::msg::MoveItErrorCodes error; const std::vector& climits = consistency_limits.empty() ? std::vector() : consistency_limits[sg]; - if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, (timeout - elapsed) / sub_groups.size(), climits, ik_sol, - error)) + if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, timeout, climits, ik_sol, error)) { std::vector solution(bij.size()); for (std::size_t i = 0; i < bij.size(); ++i) @@ -1853,6 +1877,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: found_solution = false; break; } + RCLCPP_DEBUG(LOGGER, "IK attempt: %d of %d", st, attempts); } if (found_solution) { @@ -1860,13 +1885,11 @@ 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, "Found IK solution"); return true; } } - elapsed = (ros::WallTime::now() - start).toSec(); - first_seed = false; - } while (elapsed < timeout); + } return false; } @@ -1915,10 +1938,9 @@ double RobotState::computeCartesianPath(const JointModelGroup* group, std::vecto if (max_step.translation <= 0.0 && max_step.rotation <= 0.0) { - ROS_ERROR_NAMED(LOGNAME, - "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; } @@ -1974,7 +1996,7 @@ double RobotState::computeCartesianPath(const JointModelGroup* group, std::vecto // Explicitly use a single IK attempt only: We want a smooth trajectory. // Random seeding (of additional attempts) would probably create IK jumps. - if (setFromIK(group, pose, link->getName(), consistency_limits, 0.0, validCallback, options)) + if (setFromIK(group, pose, link->getName(), consistency_limits, 1, 0.0, validCallback, options)) traj.push_back(RobotStatePtr(new RobotState(*this))); else break; @@ -2047,9 +2069,10 @@ 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 " - "Need at least %zu steps, only got %zu. Try a lower max_step.", - MIN_STEPS_FOR_JUMP_THRESH, traj.size()); + 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()); } std::vector dist_vector; @@ -2068,7 +2091,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, "Truncating Cartesian path due to detected jump in joint-space distance"); percentage = (double)(i + 1) / (double)(traj.size()); traj.resize(i + 1); break; @@ -2103,9 +2126,10 @@ double RobotState::testAbsoluteJointSpaceJump(const JointModelGroup* group, std: type_index = 1; break; default: - ROS_WARN_NAMED(LOGNAME, "Joint %s has not supported type %s. \n" - "testAbsoluteJointSpaceJump only supports prismatic and revolute joints.", - joint->getName().c_str(), joint->getTypeName().c_str()); + 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; } if (!data[type_index].check_) @@ -2114,8 +2138,8 @@ 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, - data[type_index].limit_, joint->getName().c_str()); + 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; } @@ -2174,6 +2198,58 @@ void RobotState::printStatePositions(std::ostream& out) const out << nm[i] << "=" << position_[i] << std::endl; } +void RobotState::printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out) const +{ + // TODO(davetcoleman): support joints with multiple variables / multiple DOFs such as floating joints + // TODO(davetcoleman): support unbounded joints + + const std::vector& joints = jmg->getActiveJointModels(); + + // Loop through joints + for (std::size_t i = 0; i < joints.size(); ++i) + { + // Ignore joints with more than one variable + if (joints[i]->getVariableCount() > 1) + continue; + + double current_value = getVariablePosition(joints[i]->getName()); + + // check if joint is beyond limits + bool out_of_bounds = !satisfiesBounds(joints[i]); + + const moveit::core::VariableBounds& bound = joints[i]->getVariableBounds()[0]; + + if (out_of_bounds) + out << MOVEIT_CONSOLE_COLOR_RED; + + out << " " << std::fixed << std::setprecision(5) << bound.min_position_ << "\t"; + double delta = bound.max_position_ - bound.min_position_; + double step = delta / 20.0; + + bool marker_shown = false; + for (double value = bound.min_position_; value < bound.max_position_; value += step) + { + // show marker of current value + if (!marker_shown && current_value < value) + { + out << "|"; + marker_shown = true; + } + else + out << "-"; + } + if (!marker_shown) + out << "|"; + + // show max position + out << " \t" << std::fixed << std::setprecision(5) << bound.max_position_ << " \t" << joints[i]->getName() + << " current: " << std::fixed << std::setprecision(5) << current_value << std::endl; + + if (out_of_bounds) + out << MOVEIT_CONSOLE_COLOR_RESET; + } +} + void RobotState::printDirtyInfo(std::ostream& out) const { out << " * Dirty Joint Transforms: " << std::endl; @@ -2290,7 +2366,7 @@ void getPoseString(std::ostream& ss, const Eigen::Isometry3d& pose, const std::s ss << std::endl; } } -} +} // namespace void RobotState::getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index b50f5a2b24..2666d30e0b 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2018, CITEC Bielefeld -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, CITEC Bielefeld + * 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 the 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: Robert Haschke */ #include diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 1319f2433b..c52761b80f 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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: Ioan Sucan */ #include @@ -116,7 +116,7 @@ TEST(Loading, SimpleRobot) TEST(LoadingAndFK, SimpleRobot) { moveit::core::RobotModelBuilder builder("myrobot", "base_link"); - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; tf2::toMsg(tf2::Vector3(-0.1, 0, 0), pose.position); tf2::Quaternion q; q.setRPY(0, 0, -1); @@ -579,6 +579,7 @@ std::size_t generateTestTraj(std::vectorgetJointModelGroup("base_from_base_to_e"); + ASSERT_TRUE(joint_model_group); std::vector> traj; // The full trajectory should be of length 7 @@ -594,6 +595,7 @@ TEST_F(OneRobot, testGenerateTrajectory) TEST_F(OneRobot, testAbsoluteJointSpaceJump) { const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e"); + ASSERT_TRUE(joint_model_group); std::vector> traj; // A revolute joint jumps 1.01 at the 5th waypoint and a prismatic joint jumps 1.01 at the 6th waypoint @@ -641,6 +643,7 @@ TEST_F(OneRobot, testAbsoluteJointSpaceJump) TEST_F(OneRobot, testRelativeJointSpaceJump) { const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e"); + ASSERT_TRUE(joint_model_group); std::vector> traj; // The first large jump of 1.01 occurs at the 5th waypoint so the test should trim the trajectory to length 4 @@ -671,6 +674,34 @@ TEST_F(OneRobot, testRelativeJointSpaceJump) EXPECT_NEAR(1.0, fraction, 0.01); } +TEST_F(OneRobot, testPrintCurrentPositionWithJointLimits) +{ + moveit::core::RobotState state(robot_model_); + const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e"); + ASSERT_TRUE(joint_model_group); + + state.setToDefaultValues(); + + std::cout << "\nVisual inspection should show NO joints out of bounds:" << std::endl; + state.printStatePositionsWithJointLimits(joint_model_group); + + std::cout << "\nVisual inspection should show ONE joint out of bounds:" << std::endl; + std::vector single_joint(1); + single_joint[0] = -1.0; + state.setJointPositions("joint_c", single_joint); + state.printStatePositionsWithJointLimits(joint_model_group); + + std::cout << "\nVisual inspection should show TWO joint out of bounds:" << std::endl; + single_joint[0] = 1.0; + state.setJointPositions("joint_f", single_joint); + state.printStatePositionsWithJointLimits(joint_model_group); + + std::cout << "\nVisual inspection should show ONE joint out of bounds:" << std::endl; + single_joint[0] = 0.19; + state.setJointPositions("joint_f", single_joint); + state.printStatePositionsWithJointLimits(joint_model_group); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index 0485de1e0f..a14cfcbb64 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the 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: Martin Pecka */ @@ -52,7 +52,7 @@ #endif #if VISUALIZE_PR2_RVIZ -#include +#include "rclcpp/rclcpp.hpp" #include #include #endif @@ -62,13 +62,13 @@ class TestAABB : public testing::Test protected: void SetUp() override{}; - robot_state::RobotState loadModel(const std::string robot_name) + robot_state::RobotState loadModel(const std::string& robot_name) { robot_model::RobotModelPtr model = moveit::core::loadTestingRobotModel(robot_name); return loadModel(model); } - robot_state::RobotState loadModel(robot_model::RobotModelPtr model) + robot_state::RobotState loadModel(const robot_model::RobotModelPtr& model) { robot_state::RobotState robot_state = robot_state::RobotState(model); robot_state.setToDefaultValues(); @@ -150,32 +150,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(); @@ -188,35 +190,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 @@ -234,35 +236,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 @@ -272,7 +274,7 @@ 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; + geometry_msgs::msg::Pose origin; tf2::toMsg(tf2::Vector3(0, 0, 0.051), origin.position); origin.orientation.w = 1.0; builder.addChain("base_footprint->base_link", "fixed", { origin }); @@ -305,7 +307,7 @@ 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; + geometry_msgs::msg::Pose origin; tf2::toMsg(tf2::Vector3(0, 0, 1.0), origin.position); tf2::Quaternion q; q.setRPY(0, 0, 1.5708); diff --git a/moveit_core/robot_state/test/test_kinematic_complex.cpp b/moveit_core/robot_state/test/test_kinematic_complex.cpp index 9fc0926834..e9a3d22ffa 100644 --- a/moveit_core/robot_state/test/test_kinematic_complex.cpp +++ b/moveit_core/robot_state/test/test_kinematic_complex.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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: Ioan Sucan */ @@ -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);