Added the service to control the car lights.

Added parameters to choose the desired car lights configuration with the dynamic reconfigure interface.
Changed the way the feedback messages are generated (Testing feature)

README.md

@@ -3,6 +3,8 @@
 The iri_adc_jury_server project description
 
 # ROS Interface
+### Service clients
+  - ~**set_lights** (iri_adc_msgs/set_car_lights.srv)
 ### Action servers
   - ~**adc_jury** (iri_adc_msgs/adc_jury.action)
 ### Service servers

cfg/AdcJuryServer.cfg

@@ -47,5 +47,18 @@ gen.add("block_feedback_time",  double_t,  0,         "Time in seconds to block
 gen.add("block_service",        bool_t,  0,           "Block service callback function",                     True)
 gen.add("block_service_time",   double_t,  0,         "Time in seconds to block service callback function",  3.0, 0.0, 10.0)
 
+lights_enum = gen.enum([ 
+  gen.const("off",           int_t,  0, "lights off"), 
+  gen.const("normal_drive",  int_t,  1, "normal drive"), 
+  gen.const("brake",         int_t,  2, "brake"), 
+  gen.const("turn_left",     int_t,  3, "turn left"), 
+  gen.const("turn_right",    int_t,  4, "turn right"), 
+  gen.const("parked",        int_t,  5, "parked"), 
+  gen.const("emergency",     int_t,  6, "emergency"), 
+  gen.const("ambulance",     int_t,  7, "ambulance"), 
+  gen.const("charging",      int_t,  8, "charging")],
+  "car_lights")
+
+gen.add("car_lights",   int_t,  0,         "Desired car lights configuration",  0,edit_method=lights_enum)
 
 exit(gen.generate(PACKAGE, "AdcJuryServerAlgorithm", "AdcJuryServer"))

include/adc_jury_server_alg_node.h

@@ -31,12 +31,15 @@
 // [publisher subscriber headers]
 
 // [service client headers]
+#include <iri_adc_msgs/set_car_lights.h>
 #include <iri_adc_msgs/jury_mode.h>
 
 // [action server client headers]
 #include <iri_action_server/iri_action_server.h>
 #include <iri_adc_msgs/adc_juryAction.h>
 
+#include <sstream>
+
 /**
  * \brief IRI ROS Specific Algorithm Class
  *
@@ -55,8 +58,9 @@ class AdcJuryServerAlgNode : public algorithm_base::IriBaseAlgorithm<AdcJuryServ
     void jury_mode_mutex_enter(void);
     void jury_mode_mutex_exit(void);
 
-
     // [client attributes]
+    ros::ServiceClient set_lights_client_;
+    iri_adc_msgs::set_car_lights set_lights_srv_;
 
     // [action server attributes]
     IriActionServer<iri_adc_msgs::adc_juryAction> adc_jury_aserver_;
@@ -70,9 +74,9 @@ class AdcJuryServerAlgNode : public algorithm_base::IriBaseAlgorithm<AdcJuryServ
     bool adc_jury_succeeded;
     bool adc_jury_finished;
 
-
     // [action client attributes]
 
+    std::stringstream feedback_msg;
    /**
     * \brief config variable
     *

src/adc_jury_server_alg_node.cpp

@@ -20,8 +20,8 @@ AdcJuryServerAlgNode::AdcJuryServerAlgNode(void) :
   this->jury_mode_server_ = this->private_node_handle_.advertiseService("jury_mode", &AdcJuryServerAlgNode::jury_modeCallback, this);
   pthread_mutex_init(&this->jury_mode_mutex_,NULL);
 
-  
   // [init clients]
+  set_lights_client_ = this->private_node_handle_.serviceClient<iri_adc_msgs::set_car_lights>("set_lights");
   
   // [init action servers]
   adc_jury_aserver_.registerStartCallback(boost::bind(&AdcJuryServerAlgNode::adc_juryStartCallback, this, _1));
@@ -53,7 +53,7 @@ void AdcJuryServerAlgNode::mainNodeThread(void)
   // [fill msg structures]
   
   // [fill srv structure and make request to the server]
-  
+ 
   // [fill action structure and make request to the action server]
   // To finish the action server with success
   //this->adc_jury_succeeded=true;
@@ -84,6 +84,11 @@ bool AdcJuryServerAlgNode::jury_modeCallback(iri_adc_msgs::jury_mode::Request &r
   //this->jury_mode_mutex_enter();
 
   ROS_INFO("AdcJuryServerAlgNode::jury_modeCallback: req=%d!",req.enable);
+  this->feedback_msg << "received jury mode request";
+  if(req.enable)
+    this->feedback_msg << "(enable)" << std::endl;
+  else
+    this->feedback_msg << "(disable)" << std::endl;
   //do operations with req and output on res
 
   if(this->config_.block_service)
@@ -167,10 +172,12 @@ void AdcJuryServerAlgNode::adc_juryGetResultCallback(iri_adc_msgs::adc_juryResul
 void AdcJuryServerAlgNode::adc_juryGetFeedbackCallback(iri_adc_msgs::adc_juryFeedbackPtr& feedback)
 {
   this->alg_.lock();
-  //update feedback data to be sent to client
   static int i=0;
-  feedback->status = this->config_.feedback_status_text + std::to_string(i);;
+  //update feedback data to be sent to client
+  this->feedback_msg << this->config_.feedback_status_text << i << std::endl;
+  feedback->status = this->feedback_msg.str();
   i++;
+  this->feedback_msg.str("");
   //ROS_INFO("feedback: %s", feedback->data.c_str());
   //ROS_INFO("AdcJuryServerAlgNode::adc_juryGetFeedbackCallback feedback.status='%s'", feedback->status.c_str());
   if(this->config_.block_feedback)
@@ -202,7 +209,57 @@ void AdcJuryServerAlgNode::node_config_update(Config &config, uint32_t level)
     this->adc_jury_succeeded=false;
     config.finish_action_with_failure=false;
   }
-  
+  if(config.car_lights==0)
+  {
+    set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_OFF;
+    this->feedback_msg << "set car lights in mode OFF" << std::endl;
+  }
+  else if(config.car_lights==1)
+  {
+    set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_NORMAL_DRIVE;
+    this->feedback_msg << "set car lights in mode NORMAL_DRIVE" << std::endl;
+  }
+  else if(config.car_lights==2)
+  {
+    set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_BRAKE;
+    this->feedback_msg << "set car lights in mode BRAKE" << std::endl;
+  }
+  else if(config.car_lights==3)
+  {
+    set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_TURN_LEFT;
+    this->feedback_msg << "set car lights in mode TURN_LEFT" << std::endl;
+  }
+  else if(config.car_lights==4)
+  {
+    set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_TURN_RIGHT;
+    this->feedback_msg << "set car lights in mode TURN_RIGHT" << std::endl;
+  }
+  else if(config.car_lights==5)
+  {
+    set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_PARKED;
+    this->feedback_msg << "set car lights in mode PARKED" << std::endl;
+  }
+  else if(config.car_lights==6)
+  {
+    set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_EMERGENCY;
+    this->feedback_msg << "set car lights in mode EMERGENCY" << std::endl;
+  }
+  else if(config.car_lights==7)
+  {
+    set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_AMBULANCE;
+    this->feedback_msg << "set car lights in mode AMBULANCE" << std::endl;
+  }
+  else if(config.car_lights==8)
+  {
+    set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_CHARGING;
+    this->feedback_msg << "set car lights in mode CHARGING" << std::endl;
+  }
+  ROS_INFO("AdcJuryServerAlgNode:: Calling service set_lights_client_!");
+  if (set_lights_client_.call(set_lights_srv_))
+    ROS_INFO("AdcJuryServerAlgNode:: set_lights_client_ received a response from service server");
+  else
+    ROS_INFO("AdcJuryServerAlgNode:: Failed to call service on topic %s",this->set_lights_client_.getService().c_str());
+ 
   this->config_=config;
   this->alg_.unlock();
 }