Added support to change the navigation mode using dynamic reconfigure.

cfg/LoomingModbus.cfg

@@ -43,5 +43,7 @@ gen.add("rate",            double_t,  SensorLevels.RECONFIGURE_STOP, "Main loop
 gen.add("modbus_address",  str_t,  SensorLevels.RECONFIGURE_STOP, "Modbus IP address",  "")
 gen.add("modbus_port",     int_t,  SensorLevels.RECONFIGURE_STOP, "Modbus Port",  4444,1,65536)
 gen.add("map_frame",       str_t,  SensorLevels.RECONFIGURE_STOP, "Map frame",  "")
+gen.add("call_mode_change", bool_t,  SensorLevels.RECONFIGURE_STOP, "Call mode change service",  False)
+gen.add("mode",     int_t,  SensorLevels.RECONFIGURE_STOP, "Mode to set on call mode change",  3,0,3)
 
 exit(gen.generate(PACKAGE, "LoomingModbusDriver", "LoomingModbus"))

include/looming_modbus_driver_node.h

@@ -83,6 +83,7 @@ class LoomingModbusDriverNode : public iri_base_driver::IriBaseNodeDriver<Loomin
     void move_baseActive();
     void move_baseFeedback(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
 
+    bool change_mode_call(int mode);
 
    /**
     * \brief post open hook

src/looming_modbus_driver_node.cpp

@@ -229,10 +229,46 @@ void LoomingModbusDriverNode::node_config_update(Config& new_cfg, uint32_t level
   this->driver_.lock();
   if(new_cfg.rate!=this->getRate())
     this->setRate(new_cfg.rate);
+
+  if(new_cfg.call_mode_change)
+  {
+    if(this->change_mode_call(new_cfg.mode))
+      ROS_INFO("LoomingModbusDriverNode:node_config_update: success changing mode");
+    else
+      ROS_ERROR("LoomingModbusDriverNode:node_config_update: error changing mode");
+    new_cfg.call_mode_change=false;
+  }
+
   this->config_=new_cfg;
   this->driver_.unlock();
 }
 
+bool LoomingModbusDriverNode::change_mode_call(int mode)
+{
+  bool ok=false;
+
+  this->mode_change_srv_.request.mode.mode=mode;
+  if(this->mode_change_client_.call(this->mode_change_srv_))
+  {
+    if(this->mode_change_srv_.response.accepted)
+    {
+      ROS_INFO("LoomingModbusDriverNode:: Navigation mode changed successfully");
+      ok=true;
+    }
+    else
+    {
+      ROS_WARN("LoomingModbusDriverNode:: Impossible to change navigation mode");
+      ok=false;
+    }
+  }
+  else
+  {
+    ROS_WARN_STREAM("LoomingModbusDriverNode:: Impossible to change navigation mode (no connection to " << this->mode_change_client_.getService() << ")");
+    ok=false;
+  }
+  return ok;
+}
+
 LoomingModbusDriverNode::~LoomingModbusDriverNode(void)
 {
   // [free dynamic memory]