Use generate_parameter_library to load srv kinematics parameters (mo…

…veit#1674)

moveit_kinematics/CMakeLists.txt

@@ -34,6 +34,7 @@ set(THIS_PACKAGE_LIBRARIES
   moveit_kdl_kinematics_plugin
   lma_kinematics_parameters
   moveit_lma_kinematics_plugin
+  srv_kinematics_parameters
   moveit_srv_kinematics_plugin
 )
 

moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt

@@ -1,5 +1,11 @@
 set(MOVEIT_LIB_NAME moveit_srv_kinematics_plugin)
 
+
+generate_parameter_library(
+  srv_kinematics_parameters # cmake target name for the parameter library
+  src/srv_kinematics_parameters.yaml # path to input yaml file
+)
+
 add_library(${MOVEIT_LIB_NAME} SHARED src/srv_kinematics_plugin.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
@@ -9,4 +15,8 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
   moveit_msgs
 )
 
+target_link_libraries(${MOVEIT_LIB_NAME}
+  srv_kinematics_parameters
+)
+
 install(DIRECTORY include/ DESTINATION include)

moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h

@@ -43,6 +43,7 @@
 
 // ROS2
 #include <rclcpp/rclcpp.hpp>
+#include <srv_kinematics_parameters.hpp>
 
 // System
 #include <memory>
@@ -151,5 +152,8 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase
   rclcpp::Client<moveit_msgs::srv::GetPositionIK>::SharedPtr ik_service_client_;
 
   rclcpp::Node::SharedPtr node_;
+
+  std::shared_ptr<srv_kinematics::ParamListener> param_listener_;
+  srv_kinematics::Params params_;
 };
 }  // namespace srv_kinematics_plugin

moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_parameters.yaml

@@ -0,0 +1,9 @@
+srv_kinematics:
+  kinematics_solver_service_name: {
+    type: string,
+    default_value: "solve_ik",
+    description: "Kinematics Solver Service Name",
+    validation: {
+      not_empty<>: []
+    }
+  }

moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp

@@ -64,6 +64,11 @@ bool SrvKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const
 
   RCLCPP_INFO(LOGGER, "SrvKinematicsPlugin initializing");
 
+  // Get Solver Parameters
+  std::string kinematics_param_prefix = "robot_description_kinematics." + group_name;
+  param_listener_ = std::make_shared<srv_kinematics::ParamListener>(node, kinematics_param_prefix);
+  params_ = param_listener_->get_params();
+
   storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
   joint_model_group_ = robot_model_->getJointModelGroup(group_name);
   if (!joint_model_group_)
@@ -107,18 +112,13 @@ bool SrvKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const
     ik_group_info_.link_names.push_back(tip_frame);
   }
 
-  // Choose what ROS service to send IK requests to
-  RCLCPP_DEBUG(LOGGER, "Looking for ROS service name on rosparam server with param: "
-                       "/kinematics_solver_service_name");
-  std::string ik_service_name;
-  lookupParam(node_, "kinematics_solver_service_name", ik_service_name, std::string("solve_ik"));
-
   // Setup the joint state groups that we need
   robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
   robot_state_->setToDefaultValues();
 
   // Create the ROS2 service client
-  ik_service_client_ = node_->create_client<moveit_msgs::srv::GetPositionIK>(ik_service_name);
+  RCLCPP_DEBUG(LOGGER, "IK Service client topic : %s", params_.kinematics_solver_service_name.c_str());
+  ik_service_client_ = node_->create_client<moveit_msgs::srv::GetPositionIK>(params_.kinematics_solver_service_name);
 
   if (!ik_service_client_->wait_for_service(std::chrono::seconds(1)))  // wait 0.1 seconds, blocking
     RCLCPP_WARN_STREAM(LOGGER,