Generate parameters from yaml

CMakeLists.txt

@@ -19,12 +19,17 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   tf2_ros
   tf2_kdl
   tf2_geometry_msgs
+  generate_parameter_library
 )
 
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
+generate_parameter_library(bio_ik_parameters
+  src/bio_ik_parameters.yaml
+)
+
 find_package(OpenMP)
 # the specific flag is not yet present in cmake 2.8.12
 if(OpenMP_CXX_FOUND OR OPENMP_FOUND)
@@ -119,6 +124,7 @@ target_link_libraries(
   PUBLIC
   ${FANN_LIBRARIES}
   ${OpenMP_LIBS}
+  bio_ik_parameters
 
   -static-libgcc
   -static-libstdc++
@@ -146,7 +152,7 @@ install(
   DESTINATION include
 )
 install(
-  TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugin
+  TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugin bio_ik_parameters
   EXPORT export_${PROJECT_NAME}
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib

package.xml

@@ -14,15 +14,16 @@
 
     <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
-    <depend>moveit_core</depend>
-    <depend>pluginlib</depend>
     <depend>eigen</depend>
+    <depend>generate_parameter_library</depend>
+    <depend>moveit_core</depend>
     <depend>moveit_ros_planning</depend>
+    <depend>pluginlib</depend>
     <depend>rclcpp</depend>
     <depend>tf2</depend>
     <depend>tf2_eigen</depend>
-    <depend>tf2_kdl</depend>
     <depend>tf2_geometry_msgs</depend>
+    <depend>tf2_kdl</depend>
 
     <test_depend>ament_cmake_gtest</test_depend>
 

src/bio_ik_parameters.yaml

@@ -0,0 +1,56 @@
+bio_ik:
+  mode:
+    type: string
+    default_value: "bio2_memetic"
+  profiler:
+    type: bool
+    default_value: false
+  counter:
+    type: bool
+    default_value: false
+  threads:
+    type: int
+    default_value: 0
+  random_seed:
+    type: int
+    default_value: 0
+    description: "The default value of 0 results in seeding random with std::random_device"
+  dpos:
+    type: double
+    default_value: .inf
+  drot:
+    type: double
+    default_value: .inf
+  dtwist:
+    type: double
+    default_value: 0.00001
+  no_wipeout:
+    type: bool
+    default_value: false
+  population_size:
+    type: int
+    default_value: 8
+  elite_count:
+    type: int
+    default_value: 4
+  linear_fitness:
+    type: bool
+    default_value: false
+  rotation_scale:
+    type: double
+    default_value: 0.5
+  position_only_ik:
+    type: bool
+    default_value: false
+  center_joints_weight:
+    type: double
+    default_value: 0
+  avoid_joint_limits_weight:
+    type: double
+    default_value: 0
+  minimal_displacement_weight:
+    type: double
+    default_value: 0
+
+
+

src/ik_base.h

@@ -142,7 +142,7 @@ struct IKBase : Random
     virtual void setParams(const IKParams& p) {}
 
     IKBase(const IKParams& p)
-        : Random(p.random_seed)
+        : Random(p.ros_params.random_seed)
         , model(p.robot_model)
         , modelInfo(p.robot_model)
         , params(p)

src/ik_evolution_1.cpp

@@ -132,10 +132,10 @@ struct IKEvolution1 : IKBase
 
     void setParams(const IKParams& p)
     {
-        opt_no_wipeout = p.opt_no_wipeout;
-        populationSize = p.population_size;
-        eliteCount = p.elite_count;
-        linear_fitness = p.linear_fitness;
+        opt_no_wipeout = p.ros_params.no_wipeout;
+        populationSize = p.ros_params.population_size;
+        eliteCount = p.ros_params.elite_count;
+        linear_fitness = p.ros_params.linear_fitness;
     }
 
     bool in_final_adjustment_loop;

src/ik_parallel.h

@@ -111,15 +111,15 @@ struct IKParallel
         : params(params)
     {
         // solver class name
-        std::string name = params.solver_class_name;
+        std::string name = params.ros_params.mode;
 
-        enable_counter = params.enable_counter;
+        enable_counter = params.ros_params.counter;
 
         // create solvers
         solvers.emplace_back(IKFactory::create(name, params));
         thread_count = solvers.front()->concurrency();
-        if(params.thread_count) {
-            thread_count = params.thread_count;
+        if(params.ros_params.threads != 0) {
+            thread_count = params.ros_params.threads;
         }
         while(solvers.size() < thread_count)
             solvers.emplace_back(IKFactory::clone(solvers.front().get()));

src/kinematics_plugin.cpp

@@ -114,9 +114,8 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
   mutable std::unique_ptr<moveit::core::RobotState> temp_state;
   mutable std::vector<Frame> tipFrames;
   RobotInfo robot_info;
-  bool enable_profiler;
 
-  BioIKKinematicsPlugin() { enable_profiler = false; }
+  BioIKKinematicsPlugin() { ikparams.ros_params.profiler = false; }
 
   virtual const std::vector<std::string> &getJointNames() const {
     LOG_FNC();
@@ -152,27 +151,20 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
 
   mutable std::vector<const bio_ik::Goal *> all_goals;
 
+  std::shared_ptr<bio_ik::ParamListener> param_listener;
   IKParams ikparams;
 
   mutable Problem problem;
 
-  template <class T>
-  void getRosParam(const std::string &param, T &val, const T &default_val)
-  {
-    const std::string prefix = "robot_description_kinematics." + group_name_ + ".";
-    if (!node_->has_parameter(prefix + param))
-    {
-      val = node_->declare_parameter(prefix + param, rclcpp::ParameterValue{default_val}).get<T>();
-      return;
-    }
-    val = node_->get_parameter(prefix + param).get_value<T>();
-  }
-
   bool load(std::string group_name) {
     LOG_FNC();
 
     LOG("bio ik init", node_->getName());
 
+    // Declare/get ros parameters
+    param_listener = std::make_shared<bio_ik::ParamListener>(node_);
+    ikparams.ros_params = param_listener->get_params();
+
     joint_model_group = robot_model_->getJointModelGroup(group_name);
     if (!joint_model_group) {
       LOG("failed to get joint model group");
@@ -194,32 +186,15 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
 
     link_names = tip_frames_;
 
-    // bool enable_profiler;
-    getRosParam("profiler", enable_profiler, false);
-    // if(enable_profiler) Profiler::start();
-
     robot_info = RobotInfo(robot_model_);
 
     ikparams.robot_model = robot_model_;
     ikparams.joint_model_group = joint_model_group;
 
     // initialize parameters for IKParallel
-    getRosParam("mode", ikparams.solver_class_name,
-                std::string("bio2_memetic"));
-    getRosParam("counter", ikparams.enable_counter, false);
-    getRosParam("threads", ikparams.thread_count, 0);
-    getRosParam("random_seed", ikparams.random_seed, static_cast<int>(std::random_device()()));
-
-    // initialize parameters for Problem
-    getRosParam("dpos", ikparams.dpos, DBL_MAX);
-    getRosParam("drot", ikparams.drot, DBL_MAX);
-    getRosParam("dtwist", ikparams.dtwist, 1e-5);
-
-    // initialize parameters for ik_evolution_1
-    getRosParam("no_wipeout", ikparams.opt_no_wipeout, false);
-    getRosParam("population_size", ikparams.population_size, 8);
-    getRosParam("elite_count", ikparams.elite_count, 4);
-    getRosParam("linear_fitness", ikparams.linear_fitness, false);
+    if (ikparams.ros_params.random_seed == 0) {
+      ikparams.ros_params.random_seed = static_cast<int>(std::random_device()());
+    }
 
     temp_state.reset(new moveit::core::RobotState(robot_model_));
 
@@ -238,12 +213,9 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
 
         // LOG_VAR(goal->link_name);
 
-        double rotation_scale = 0.5;
-
-        getRosParam("rotation_scale", rotation_scale, rotation_scale);
+        double rotation_scale = ikparams.ros_params.rotation_scale;
+        bool position_only_ik = ikparams.ros_params.position_only_ik;
 
-        bool position_only_ik = false;
-        getRosParam("position_only_ik", position_only_ik, position_only_ik);
         if (position_only_ik)
           rotation_scale = 0;
 
@@ -253,8 +225,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
       }
 
       {
-        double weight = 0;
-        getRosParam("center_joints_weight", weight, weight);
+        double weight = ikparams.ros_params.center_joints_weight;
         if (weight > 0.0) {
           auto *center_joints_goal = new bio_ik::CenterJointsGoal();
           center_joints_goal->setWeight(weight);
@@ -263,8 +234,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
       }
 
       {
-        double weight = 0;
-        getRosParam("avoid_joint_limits_weight", weight, weight);
+        double weight = ikparams.ros_params.avoid_joint_limits_weight;
         if (weight > 0.0) {
           auto *avoid_joint_limits_goal = new bio_ik::AvoidJointLimitsGoal();
           avoid_joint_limits_goal->setWeight(weight);
@@ -273,8 +243,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
       }
 
       {
-        double weight = 0;
-        getRosParam("minimal_displacement_weight", weight, weight);
+        double weight = ikparams.ros_params.minimal_displacement_weight;
         if (weight > 0.0) {
           auto *minimal_displacement_goal =
               new bio_ik::MinimalDisplacementGoal();
@@ -395,7 +364,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
                    const moveit::core::RobotState *context_state = NULL) const {
     std::chrono::time_point<std::chrono::system_clock, std::chrono::duration<double>> t0 = std::chrono::system_clock::now();
 
-    if (enable_profiler)
+    if (ikparams.ros_params.profiler)
       Profiler::start();
 
     auto *bio_ik_options = toBioIKKinematicsQueryOptions(&options);

src/problem.cpp

@@ -87,9 +87,9 @@ void Problem::initialize(moveit::core::RobotModelConstPtr robot_model, const mov
     if(!ros_params_initrd)
     {
         ros_params_initrd = true;
-        dpos = params.dpos;
-        drot = params.drot;
-        dtwist = params.dtwist;
+        dpos = params.ros_params.dpos;
+        drot = params.ros_params.drot;
+        dtwist = params.ros_params.dtwist;
         if(dpos < 0.0 || dpos >= FLT_MAX || !std::isfinite(dpos)) dpos = DBL_MAX;
         if(drot < 0.0 || drot >= FLT_MAX || !std::isfinite(drot)) drot = DBL_MAX;
         if(dtwist < 0.0 || dtwist >= FLT_MAX || !std::isfinite(dtwist)) dtwist = DBL_MAX;

src/utils.h

@@ -53,6 +53,8 @@
 //#include <Eigen/Eigen>
 #include <moveit/robot_model/robot_model.h>
 
+#include "bio_ik_parameters.hpp"
+
 namespace bio_ik
 {
 
@@ -61,22 +63,8 @@ struct IKParams
     moveit::core::RobotModelConstPtr robot_model;
     const moveit::core::JointModelGroup* joint_model_group;
 
-    // IKParallel parameters
-    std::string solver_class_name;
-    bool enable_counter;
-    int thread_count;
-    int random_seed;
-
-    //Problem parameters
-    double dpos;
-    double drot;
-    double dtwist;
-
-    // ik_evolution_1 parameters
-    bool opt_no_wipeout;
-    int population_size;
-    int elite_count;
-    bool linear_fitness;
+    // ROS Parameters
+    Params ros_params;
 };
 
 // Uncomment to enable logging

upstream.repos

@@ -0,0 +1,13 @@
+repositories:
+  generate_parameter_library:
+    type: git
+    url: https://github.com/PickNikRobotics/generate_parameter_library.git
+    version: main
+  moveit2:
+    type: git
+    url: https://github.com/ros-planning/moveit2.git
+    version: main
+  ros2_control:
+    type: git
+    url: https://github.com/ros-controls/ros2_control.git
+    version: master