Use class bool variable instead of static bool to update motion_group names,...

Use class bool variable instead of static bool to update motion_group names, so it's done for multiple instances of the module

config/ari_arm_module_default.yaml

@@ -17,7 +17,7 @@ max_plan_time: 10.0
 max_vel_scale: 1.0
 end_effector_tool: "arm_left_grasping_frame"
 
-ws_frame_id: "/base_footprint"
+ws_frame_id: "base_footprint"
 ws_x_min: -1.0
 ws_y_min: -1.0
 ws_z_min: -1.0

config/ivo_arm_module_default.yaml

@@ -17,7 +17,7 @@ max_plan_time: 10.0
 max_vel_scale: 1.0
 end_effector_tool: "arm_left_tool_link"
 
-ws_frame_id: "/base_footprint"
+ws_frame_id: "base_footprint"
 ws_x_min: -1.0
 ws_y_min: -1.0
 ws_z_min: -1.0

config/tiago_arm_module_default.yaml

@@ -17,7 +17,7 @@ max_plan_time: 10.0
 max_vel_scale: 1.0
 end_effector_tool: "arm_tool_link"
 
-ws_frame_id: "/base_footprint"
+ws_frame_id: "base_footprint"
 ws_x_min: -1.0
 ws_y_min: -1.0
 ws_z_min: -1.0

config/tiago_dual_arm_module_default.yaml

@@ -17,7 +17,7 @@ max_plan_time: 10.0
 max_vel_scale: 1.0
 end_effector_tool: "arm_left_tool_link"
 
-ws_frame_id: "/base_footprint"
+ws_frame_id: "base_footprint"
 ws_x_min: -1.0
 ws_y_min: -1.0
 ws_z_min: -1.0

include/tiago_arm_module/tiago_arm_module.h

@@ -64,6 +64,7 @@ class CTiagoArmModule : public CModule<tiago_arm_module::TiagoArmModuleConfig>
     tiago_arm_module_status_t status;
     bool new_motion;
     bool cancel_pending;
+    bool first_dynamic_reconfigure_call;
 
     // planning objects
     std::vector<TVisualObject> objects;

src/tiago_arm_module.cpp

@@ -4,6 +4,8 @@ CTiagoArmModule::CTiagoArmModule(const std::string &name,const std::string &name
   arm_action("arm",this->module_nh.getNamespace()),
   get_scene("get_planning_scene", this->module_nh.getNamespace())
 {
+  this->first_dynamic_reconfigure_call=true;
+
   this->start_operation();
 
   // initialize odometry subscriber
@@ -105,29 +107,34 @@ void CTiagoArmModule::state_machine(void)
 
 void CTiagoArmModule::reconfigure_callback(tiago_arm_module::TiagoArmModuleConfig &config, uint32_t level)
 {
-  static bool first=true;
-
-  ROS_INFO("CTiagoArmModule : reconfigure callback");
+  ROS_INFO("CTiagoArmModule : reconfigure callback: %s", this->module_nh.getNamespace().c_str());
   this->lock();
   this->config=config;
   /* set the module rate */
-  this->dynamic_reconfigure(config,"arm_module");
+
+  this->dynamic_reconfigure(config,"arm");
   /* motion action parameters */
   this->arm_action.dynamic_reconfigure(config,"move");
-  // handle the move groups
-  if(first)
+
+  if(this->first_dynamic_reconfigure_call)
   {
+    // handle the move groups
     this->move_groups.clear();
     this->move_groups=this->get_strings(this->module_nh,"move_groups");
     this->move_group_joints.resize(this->move_groups.size());
     for(unsigned int i=0;i<this->move_groups.size();i++)
+    {
       this->move_group_joints[i]=this->get_strings(this->module_nh,this->move_groups[i]);
-    // HACK: it was impossible to use a parameter call arm with all the joints, so the arm_only name
-    for(unsigned int i=0;i<this->move_groups.size();i++)
-      if(this->move_groups[i]=="arm_only")
-        this->move_groups[i]=="arm";
-    first=false;
+      //ROS_INFO("CTiagoArmModule: move_groups[i]=%s",this->move_groups[i].c_str());
+      
+      // HACK: it was impossible to use a parameter called 'arm' with all the joints, so the 'arm_only' name is used
+      //if(this->move_groups[i]=="arm_only")
+      //  this->move_groups[i]=="arm";
+    }
+
+    this->first_dynamic_reconfigure_call=false;
   }
+
   this->unlock();
   // planner params
   this->set_planner(config.planner_id);