diff --git a/README.md b/README.md
index 0c0a47bc3a151efdd0eeac909d859e661bf18f8b..2cb59d2005ed102b201eb6e668d98f99522ba945 100644
--- a/README.md
+++ b/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
diff --git a/cfg/AdcJuryServer.cfg b/cfg/AdcJuryServer.cfg
index 9c8ea6a1a71d9d4c90b2de10b9018a310dd5d228..fe4edc4c345c595549e754709b136cf187290c51 100755
--- a/cfg/AdcJuryServer.cfg
+++ b/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"))
diff --git a/include/adc_jury_server_alg_node.h b/include/adc_jury_server_alg_node.h
index 03102820715825c5efc3133c57e1f41c7cfcfed0..da29f8c62d55cd24c727f690477afc55771ff37a 100644
--- a/include/adc_jury_server_alg_node.h
+++ b/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
*
diff --git a/src/adc_jury_server_alg_node.cpp b/src/adc_jury_server_alg_node.cpp
index f739613cad4d757748ebc9269c8a7f0681e1fa8a..d9d0ca072ce3d7ffccef5ff2ba522307bcceeb33 100644
--- a/src/adc_jury_server_alg_node.cpp
+++ b/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();
}