From 4ec5ba29f2db86abfa9b599c18660a90a21a308f Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Mon, 10 Feb 2025 11:39:40 +0100
Subject: [PATCH] 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 | 2 +-
config/ivo_arm_module_default.yaml | 2 +-
config/tiago_arm_module_default.yaml | 2 +-
config/tiago_dual_arm_module_default.yaml | 2 +-
include/tiago_arm_module/tiago_arm_module.h | 1 +
src/tiago_arm_module.cpp | 29 +++++++++++++--------
6 files changed, 23 insertions(+), 15 deletions(-)
diff --git a/config/ari_arm_module_default.yaml b/config/ari_arm_module_default.yaml
index ba28a4c..dab2787 100644
--- a/config/ari_arm_module_default.yaml
+++ b/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
diff --git a/config/ivo_arm_module_default.yaml b/config/ivo_arm_module_default.yaml
index 3d8ab3f..ad11e42 100644
--- a/config/ivo_arm_module_default.yaml
+++ b/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
diff --git a/config/tiago_arm_module_default.yaml b/config/tiago_arm_module_default.yaml
index a1ff2a3..85b1888 100644
--- a/config/tiago_arm_module_default.yaml
+++ b/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
diff --git a/config/tiago_dual_arm_module_default.yaml b/config/tiago_dual_arm_module_default.yaml
index 3d8ab3f..ad11e42 100644
--- a/config/tiago_dual_arm_module_default.yaml
+++ b/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
diff --git a/include/tiago_arm_module/tiago_arm_module.h b/include/tiago_arm_module/tiago_arm_module.h
index 4cd07cc..6bcef2d 100644
--- a/include/tiago_arm_module/tiago_arm_module.h
+++ b/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;
diff --git a/src/tiago_arm_module.cpp b/src/tiago_arm_module.cpp
index 29c3cfd..4d7e8a8 100644
--- a/src/tiago_arm_module.cpp
+++ b/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);
--
GitLab