Port kinematics_base submodule moveit_core

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})
+# add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+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)

moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h

@@ -37,14 +37,16 @@
 #ifndef MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_
 #define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_
 
-#include <geometry_msgs/PoseStamped.h>
-#include <moveit_msgs/MoveItErrorCodes.h>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <moveit_msgs/msg/move_it_error_codes.hpp>
 #include <moveit/macros/class_forward.h>
 #include <moveit/macros/deprecation.h>
-#include <ros/node_handle.h>
+#include "rclcpp/rclcpp.hpp"
 
 #include <boost/function.hpp>
 #include <string>
+#include <moveit/logging/logging.h>
 
 namespace moveit
 {
@@ -137,7 +139,7 @@ struct KinematicsResult
                                        of solutions explored. */
 };
 
-MOVEIT_CLASS_FORWARD(KinematicsBase);
+MOVEIT_CLASS_FORWARD(KinematicsBase)
 
 /**
  * @class KinematicsBase
@@ -149,9 +151,9 @@ class KinematicsBase
   static const double DEFAULT_SEARCH_DISCRETIZATION; /* = 0.1 */
   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<void(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_solution,
-                               moveit_msgs::MoveItErrorCodes& error_code)>
+  /** @brief The signature for a callback that can compute IK */
+  typedef boost::function<void(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_solution,
+                               moveit_msgs::msg::MoveItErrorCodes& error_code)>
       IKCallbackFn;
 
   /**
@@ -167,8 +169,8 @@ class KinematicsBase
    * @return True if a valid solution was found, false otherwise
    */
   virtual bool
-  getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
-                std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
+  getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
+                std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
                 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
 
   /**
@@ -186,7 +188,7 @@ 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<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
+  virtual bool getPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
                              std::vector<std::vector<double> >& solutions, KinematicsResult& result,
                              const kinematics::KinematicsQueryOptions& options) const;
 
@@ -203,8 +205,8 @@ class KinematicsBase
    * @return True if a valid solution was found, false otherwise
    */
   virtual bool
-  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
-                   std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
+  searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
+                   std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
                    const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
 
   /**
@@ -222,9 +224,9 @@ class KinematicsBase
    * @return True if a valid solution was found, false otherwise
    */
   virtual bool
-  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
+  searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
                    const std::vector<double>& consistency_limits, std::vector<double>& solution,
-                   moveit_msgs::MoveItErrorCodes& error_code,
+                   moveit_msgs::msg::MoveItErrorCodes& error_code,
                    const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
 
   /**
@@ -241,9 +243,9 @@ class KinematicsBase
    * @return True if a valid solution was found, false otherwise
    */
   virtual bool
-  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
+  searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
                    std::vector<double>& solution, const IKCallbackFn& solution_callback,
-                   moveit_msgs::MoveItErrorCodes& error_code,
+                   moveit_msgs::msg::MoveItErrorCodes& error_code,
                    const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
 
   /**
@@ -262,9 +264,9 @@ class KinematicsBase
    * @return True if a valid solution was found, false otherwise
    */
   virtual bool
-  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
+  searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
                    const std::vector<double>& consistency_limits, std::vector<double>& solution,
-                   const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
+                   const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
                    const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
 
   /**
@@ -289,9 +291,9 @@ class KinematicsBase
    * @return True if a valid solution was found, false otherwise
    */
   virtual bool
-  searchPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
+  searchPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
                    double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
-                   const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
+                   const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
                    const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
                    const moveit::core::RobotState* context_state = NULL) const
   {
@@ -323,7 +325,7 @@ class KinematicsBase
    * @return True if a valid solution was found, false otherwise
    */
   virtual bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
-                             std::vector<geometry_msgs::Pose>& poses) const = 0;
+                             std::vector<geometry_msgs::msg::Pose>& poses) const = 0;
 
   /**
    * @brief Set the parameters for the solver, for use with kinematic chain IK solvers
@@ -612,33 +614,74 @@ class KinematicsBase
   template <typename T>
   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;
+    // ros::NodeHandle pnh("~");
+
+    auto node = rclcpp::Node::make_shared("lookupparam");
+
+    auto parameters_lookup = std::make_shared<rclcpp::SyncParametersClient>(node);
+
+    auto groupname_param = parameters_lookup->get_parameters({group_name_ + "/" + param});
+
+    for (auto & parameter : groupname_param) {
+        if (!parameter.get_name().compare(group_name_ + "/" + param)) {
+          val = parameter.value_to_string();
+          return true;
+        }
     }
 
-    if (pnh.hasParam(param))
-    {
-      val = pnh.param(param, default_val);
-      return true;
+    auto only_param = parameters_lookup->get_parameters({param});
+
+    for (auto & parameter : only_param) {
+        if (!parameter.get_name().compare(param)) {
+          val = parameter.value_to_string();
+          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;
+    auto robot_description_groupname_kinematics_param = parameters_lookup->get_parameters({"robot_description_kinematics/" + group_name_ + "/" + param});
+
+    for (auto & parameter : robot_description_groupname_kinematics_param) {
+        if (!parameter.get_name().compare("robot_description_kinematics/" + group_name_ + "/" + param)) {
+          val = parameter.value_to_string();
+          return true;
+        }
     }
 
-    if (nh.hasParam("robot_description_kinematics/" + param))
-    {
-      val = nh.param("robot_description_kinematics/" + param, default_val);
-      return true;
+    auto robot_description_kinematics_param = parameters_lookup->get_parameters({"robot_description_kinematics/" + param});
+
+    for (auto & parameter : robot_description_kinematics_param) {
+        if (!parameter.get_name().compare("robot_description_kinematics/" + param)) {
+          val = parameter.value_to_string();
+          return true;
+        }
     }
 
-    val = default_val;
+    // if (pnh.hasParam(group_name_ + "/" + param))
+    // {
+    //   val = pnh.param(group_name_ + "/" + param, default_val);
+    //   return true;
+    // }
+    //
+    // if (pnh.hasParam(param))
+    // {
+    //   val = pnh.param(param, default_val);
+    //   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 +701,6 @@ class KinematicsBase
 private:
   std::string removeSlash(const std::string& str) const;
 };
-};
+}
 
 #endif

moveit_core/kinematics_base/src/kinematics_base.cpp

@@ -112,15 +112,15 @@ 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");
+  ROS_ERROR_NAMED(LOGNAME.c_str(), "This solver does not support multiple tip frames");
   return false;
 }
 
 bool KinematicsBase::initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name,
                                 const std::string& base_frame, const std::vector<std::string>& tip_frames,
                                 double search_discretization)
 {
-  ROS_WARN_NAMED(LOGNAME, "IK plugin for group '%s' relies on deprecated API. "
+  ROS_WARN_NAMED(LOGNAME.c_str(), "IK plugin for group '%s' relies on deprecated API. "
                           "Please implement initialize(RobotModel, ...).",
                  group_name.c_str());
   return false;
@@ -190,7 +190,7 @@ KinematicsBase::~KinematicsBase()
 {
 }
 
-bool KinematicsBase::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
+bool KinematicsBase::getPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses,
                                    const std::vector<double>& ik_seed_state,
                                    std::vector<std::vector<double> >& solutions, KinematicsResult& result,
                                    const KinematicsQueryOptions& options) const
@@ -207,19 +207,19 @@ bool KinematicsBase::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_po
 
   if (ik_poses.size() != 1)
   {
-    ROS_ERROR_NAMED(LOGNAME, "This kinematic solver does not support getPositionIK for multiple tips");
+    ROS_ERROR_NAMED(LOGNAME.c_str(), "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");
+    ROS_ERROR_NAMED(LOGNAME.c_str(), "Input ik_poses array is empty");
     result.kinematic_error = KinematicErrors::EMPTY_TIP_POSES;
     return false;
   }
 
-  moveit_msgs::MoveItErrorCodes error_code;
+  moveit_msgs::msg::MoveItErrorCodes error_code;
   if (getPositionIK(ik_poses[0], ik_seed_state, solution, error_code, options))
   {
     solutions.resize(1);