diff --git a/moveit_core/kinematics_base/CMakeLists.txt b/moveit_core/kinematics_base/CMakeLists.txt index f4d98357ef..d44a2d1baa 100644 --- a/moveit_core/kinematics_base/CMakeLists.txt +++ b/moveit_core/kinematics_base/CMakeLists.txt @@ -1,18 +1,19 @@ +cmake_minimum_required(VERSION 3.5) set(MOVEIT_LIB_NAME moveit_kinematics_base) add_library(${MOVEIT_LIB_NAME} src/kinematics_base.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + # Avoid warnings about deprecated members of KinematicsBase when building KinematicsBase itself. # TODO: remove when deperecated variables (tip_frame_, search_discretization_) are finally removed. -target_compile_options(${MOVEIT_LIB_NAME} PRIVATE -Wno-deprecated-declarations) +# target_compile_options(${MOVEIT_LIB_NAME} PRIVATE -Wno-deprecated-declarations) # This line is needed to ensure that messages are done being built before this is built -add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) +ament_target_dependencies(${MOVEIT_LIB_NAME} urdf urdfdom_headers) install(TARGETS ${MOVEIT_LIB_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +install(DIRECTORY include/ + DESTINATION include) diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 9f9bef006e..44331bee2f 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -37,12 +37,11 @@ #ifndef MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ #define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ -#include +#include #include #include #include -#include - +#include "rclcpp/rclcpp.hpp" #include #include @@ -53,12 +52,15 @@ namespace core MOVEIT_CLASS_FORWARD(JointModelGroup) MOVEIT_CLASS_FORWARD(RobotState) MOVEIT_CLASS_FORWARD(RobotModel) -} -} +} // namespace core +} // namespace moveit /** @brief API for forward and inverse kinematics */ namespace kinematics { +// Logger +rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_base"); + /* * @enum DiscretizationMethods * @@ -137,7 +139,7 @@ struct KinematicsResult of solutions explored. */ }; -MOVEIT_CLASS_FORWARD(KinematicsBase); +MOVEIT_CLASS_FORWARD(KinematicsBase) /** * @class KinematicsBase @@ -150,7 +152,8 @@ class KinematicsBase static const double DEFAULT_TIMEOUT; /* = 1.0 */ /** @brief Signature for a callback to validate an IK solution. Typically used for collision checking. */ - typedef boost::function& ik_solution, + /** @brief The signature for a callback that can compute IK */ + typedef boost::function& ik_solution, moveit_msgs::msg::MoveItErrorCodes& error_code)> IKCallbackFn; @@ -167,7 +170,7 @@ class KinematicsBase * @return True if a valid solution was found, false otherwise */ virtual bool - getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, + getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; @@ -186,9 +189,9 @@ class KinematicsBase * other will result in failure. * @return True if a valid set of solutions was found, false otherwise. */ - virtual bool getPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, - std::vector >& solutions, KinematicsResult& result, - const kinematics::KinematicsQueryOptions& options) const; + virtual bool getPositionIK(const std::vector& ik_poses, + const std::vector& ik_seed_state, std::vector >& solutions, + KinematicsResult& result, const kinematics::KinematicsQueryOptions& options) const; /** * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. @@ -203,7 +206,7 @@ class KinematicsBase * @return True if a valid solution was found, false otherwise */ virtual bool - searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; @@ -222,7 +225,7 @@ class KinematicsBase * @return True if a valid solution was found, false otherwise */ virtual bool - searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limits, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; @@ -241,7 +244,7 @@ class KinematicsBase * @return True if a valid solution was found, false otherwise */ virtual bool - searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; @@ -262,7 +265,7 @@ class KinematicsBase * @return True if a valid solution was found, false otherwise */ virtual bool - searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; @@ -289,7 +292,7 @@ class KinematicsBase * @return True if a valid solution was found, false otherwise */ virtual bool - searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, + searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), @@ -311,7 +314,7 @@ class KinematicsBase } // Otherwise throw error because this function should have been implemented - ROS_ERROR_NAMED("kinematics_base", "This kinematic solver does not support searchPositionIK with multiple poses"); + RCLCPP_ERROR(LOGGER, "This kinematic solver does not support searchPositionIK with multiple poses"); return false; } @@ -323,7 +326,7 @@ class KinematicsBase * @return True if a valid solution was found, false otherwise */ virtual bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, - std::vector& poses) const = 0; + std::vector& poses) const = 0; /** * @brief Set the parameters for the solver, for use with kinematic chain IK solvers @@ -435,8 +438,8 @@ class KinematicsBase virtual const std::string& getTipFrame() const { if (tip_frames_.size() > 1) - ROS_ERROR_NAMED("kinematics_base", "This kinematic solver has more than one tip frame, " - "do not call getTipFrame()"); + RCLCPP_ERROR(LOGGER, "This kinematic solver has more than one tip frame, " + "do not call getTipFrame()"); return tip_frames_[0]; } @@ -612,34 +615,17 @@ class KinematicsBase template inline bool lookupParam(const std::string& param, T& val, const T& default_val) const { - ros::NodeHandle pnh("~"); - if (pnh.hasParam(group_name_ + "/" + param)) - { - val = pnh.param(group_name_ + "/" + param, default_val); - return true; - } - - if (pnh.hasParam(param)) + // TODO(henningkayser): reuse node and fix private namespace lookup from kinematics.yaml + auto node = rclcpp::Node::make_shared("kinematics_base.lookup_param"); + std::vector param_results = node->get_parameters( + { group_name_ + "/" + param, param, "robot_description_kinematics/" + group_name_ + "/" + param, + "robot_description_kinematics/" + param }); + if (!param_results.empty()) { - val = pnh.param(param, default_val); + val = param_results[0].get_value(); return true; } - - ros::NodeHandle nh; - if (nh.hasParam("robot_description_kinematics/" + group_name_ + "/" + param)) - { - val = nh.param("robot_description_kinematics/" + group_name_ + "/" + param, default_val); - return true; - } - - if (nh.hasParam("robot_description_kinematics/" + param)) - { - val = nh.param("robot_description_kinematics/" + param, default_val); - return true; - } - val = default_val; - return false; } @@ -658,6 +644,6 @@ class KinematicsBase private: std::string removeSlash(const std::string& str) const; }; -}; +} // namespace kinematics #endif diff --git a/moveit_core/kinematics_base/src/kinematics_base.cpp b/moveit_core/kinematics_base/src/kinematics_base.cpp index 29b2dfb707..a496c596bc 100644 --- a/moveit_core/kinematics_base/src/kinematics_base.cpp +++ b/moveit_core/kinematics_base/src/kinematics_base.cpp @@ -37,8 +37,6 @@ #include #include -static const std::string LOGNAME = "kinematics_base"; - namespace kinematics { const double KinematicsBase::DEFAULT_SEARCH_DISCRETIZATION = 0.1; @@ -112,7 +110,7 @@ bool KinematicsBase::initialize(const std::string& robot_description, const std: return initialize(robot_description, group_name, base_frame, tip_frames[0], search_discretization); } - ROS_ERROR_NAMED(LOGNAME, "This solver does not support multiple tip frames"); + RCLCPP_ERROR(LOGGER, "This solver does not support multiple tip frames"); return false; } @@ -120,8 +118,9 @@ bool KinematicsBase::initialize(const moveit::core::RobotModel& robot_model, con const std::string& base_frame, const std::vector& tip_frames, double search_discretization) { - ROS_WARN_NAMED(LOGNAME, "IK plugin for group '%s' relies on deprecated API. " - "Please implement initialize(RobotModel, ...).", + RCLCPP_WARN(LOGGER, + "IK plugin for group '%s' relies on deprecated API. " + "Please implement initialize(RobotModel, ...).", group_name.c_str()); return false; } @@ -188,7 +187,7 @@ KinematicsBase::KinematicsBase() KinematicsBase::~KinematicsBase() = default; -bool KinematicsBase::getPositionIK(const std::vector& ik_poses, +bool KinematicsBase::getPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, std::vector >& solutions, KinematicsResult& result, const KinematicsQueryOptions& options) const @@ -205,14 +204,14 @@ bool KinematicsBase::getPositionIK(const std::vector& ik_po if (ik_poses.size() != 1) { - ROS_ERROR_NAMED(LOGNAME, "This kinematic solver does not support getPositionIK for multiple tips"); + RCLCPP_ERROR(LOGGER, "This kinematic solver does not support getPositionIK for multiple tips"); result.kinematic_error = KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED; return false; } if (ik_poses.empty()) { - ROS_ERROR_NAMED(LOGNAME, "Input ik_poses array is empty"); + RCLCPP_ERROR(LOGGER, "Input ik_poses array is empty"); result.kinematic_error = KinematicErrors::EMPTY_TIP_POSES; return false; }