Merge pull request #714 from henhenhen/kinetic-devel_lookup-param

Use lookupParam() in kinematics plugins

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

@@ -602,17 +602,27 @@ class KinematicsBase
 
   /**
    * @brief Enables kinematics plugins access to parameters that are defined
-   * for the 'robot_description_kinematics' namespace.
-   * Parameters are queried in order of the specified group hierarchy.
-   * That is parameters are first searched in the private namespace
-   * then in the subroup namespace and finally in the group namespace.
+   * for the private namespace and inside 'robot_description_kinematics'.
+   * Parameters are searched in the following locations and order
+   *
+   * ~/<group_name>/<param>
+   * ~/<param>
+   * robot_description_kinematics/<group_name>/<param>
+   * robot_description_kinematics/<param>
+   *
    * This order maintains default behavior by keeping the private namespace
    * as the predominant configuration but also allows groupwise specifications.
    */
   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;
+    }
+
     if (pnh.hasParam(param))
     {
       val = pnh.param(param, default_val);
@@ -632,6 +642,8 @@ class KinematicsBase
       return true;
     }
 
+    val = default_val;
+
     return false;
   }
 

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

@@ -344,7 +344,7 @@ bool IKFastKinematicsPlugin::initialize(const std::string& robot_description, co
   ros::NodeHandle node_handle("~/" + group_name);
 
   std::string robot;
-  node_handle.param("robot", robot, std::string());
+  lookupParam("robot", robot, std::string());
 
   // IKFast56/61
   fillFreeParams(GetNumFreeParameters(), GetFreeParameters());
@@ -366,7 +366,7 @@ bool IKFastKinematicsPlugin::initialize(const std::string& robot_description, co
   std::string xml_string;
 
   std::string urdf_xml, full_urdf_xml;
-  node_handle.param("urdf_xml", urdf_xml, robot_description);
+  lookupParam("urdf_xml", urdf_xml, robot_description);
   node_handle.searchParam(urdf_xml, full_urdf_xml);
 
   ROS_DEBUG_NAMED("ikfast", "Reading xml file from parameter server");
@@ -376,7 +376,6 @@ bool IKFastKinematicsPlugin::initialize(const std::string& robot_description, co
     return false;
   }
 
-  node_handle.param(full_urdf_xml, xml_string, std::string());
   robot_model.initString(xml_string);
 
   ROS_DEBUG_STREAM_NAMED("ikfast", "Reading joints and links from URDF");

moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp

@@ -131,7 +131,6 @@ bool KDLKinematicsPlugin::initialize(const std::string& robot_description, const
 {
   setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
 
-  ros::NodeHandle private_handle("~");
   rdf_loader::RDFLoader rdf_loader(robot_description_);
   const srdf::ModelSharedPtr& srdf = rdf_loader.getSRDF();
   const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.getURDF();
@@ -210,11 +209,10 @@ bool KDLKinematicsPlugin::initialize(const std::string& robot_description, const
   double epsilon;
   bool position_ik;
 
-  private_handle.param("max_solver_iterations", max_solver_iterations, 500);
-  private_handle.param("epsilon", epsilon, 1e-5);
-  private_handle.param(group_name + "/position_only_ik", position_ik, false);
-  ROS_DEBUG_NAMED("kdl", "Looking in private handle: %s for param name: %s", private_handle.getNamespace().c_str(),
-                  (group_name + "/position_only_ik").c_str());
+  lookupParam("max_solver_iterations", max_solver_iterations, 500);
+  lookupParam("epsilon", epsilon, 1e-5);
+  lookupParam("position_only_ik", position_ik, false);
+  ROS_DEBUG_NAMED("kdl", "Looking for param name: position_only_ik");
 
   if (position_ik)
     ROS_INFO_NAMED("kdl", "Using position only ik");

moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp

@@ -130,7 +130,6 @@ bool LMAKinematicsPlugin::initialize(const std::string& robot_description, const
 {
   setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
 
-  ros::NodeHandle private_handle("~");
   rdf_loader::RDFLoader rdf_loader(robot_description_);
   const srdf::ModelSharedPtr& srdf = rdf_loader.getSRDF();
   const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.getURDF();
@@ -209,11 +208,10 @@ bool LMAKinematicsPlugin::initialize(const std::string& robot_description, const
   double epsilon;
   bool position_ik;
 
-  private_handle.param("max_solver_iterations", max_solver_iterations, 500);
-  private_handle.param("epsilon", epsilon, 1e-5);
-  private_handle.param(group_name + "/position_only_ik", position_ik, false);
-  ROS_DEBUG_NAMED("lma", "Looking in private handle: %s for param name: %s", private_handle.getNamespace().c_str(),
-                  (group_name + "/position_only_ik").c_str());
+  lookupParam("max_solver_iterations", max_solver_iterations, 500);
+  lookupParam("epsilon", epsilon, 1e-5);
+  lookupParam("position_only_ik", position_ik, false);
+  ROS_DEBUG_NAMED("lma", "Looking for param name: position_only_ik");
 
   if (position_ik)
     ROS_INFO_NAMED("lma", "Using position only ik");

moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp

@@ -67,7 +67,6 @@ bool SrvKinematicsPlugin::initialize(const std::string& robot_description, const
 
   setValues(robot_description, group_name, base_frame, tip_frames, search_discretization);
 
-  ros::NodeHandle private_handle("~");
   rdf_loader::RDFLoader rdf_loader(robot_description_);
   const srdf::ModelSharedPtr& srdf = rdf_loader.getSRDF();
   const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.getURDF();
@@ -124,11 +123,10 @@ bool SrvKinematicsPlugin::initialize(const std::string& robot_description, const
   }
 
   // Choose what ROS service to send IK requests to
-  ROS_DEBUG_STREAM_NAMED("srv", "Looking for ROS service name on rosparm server at location: "
-                                    << private_handle.getNamespace() << "/" << group_name_
+  ROS_DEBUG_STREAM_NAMED("srv", "Looking for ROS service name on rosparam server with param: "
                                     << "/kinematics_solver_service_name");
   std::string ik_service_name;
-  private_handle.param(group_name_ + "/kinematics_solver_service_name", ik_service_name, std::string("solve_ik"));
+  lookupParam("kinematics_solver_service_name", ik_service_name, std::string("solve_ik"));
 
   // Setup the joint state groups that we need
   robot_state_.reset(new robot_state::RobotState(robot_model_));