diff --git a/cfg/AdcJuryServer.cfg b/cfg/AdcJuryServer.cfg
index fe4edc4c345c595549e754709b136cf187290c51..e1a998e4448aabf5530e675033a5fb00f8f243fa 100755
--- a/cfg/AdcJuryServer.cfg
+++ b/cfg/AdcJuryServer.cfg
@@ -55,8 +55,7 @@ lights_enum = gen.enum([
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")],
+ gen.const("ambulance", int_t, 7, "ambulance")],
"car_lights")
gen.add("car_lights", int_t, 0, "Desired car lights configuration", 0,edit_method=lights_enum)
diff --git a/include/adc_jury_server_alg_node.h b/include/adc_jury_server_alg_node.h
index da29f8c62d55cd24c727f690477afc55771ff37a..ff143dd4cacfef9462c53be75de0fd93dbb93588 100644
--- a/include/adc_jury_server_alg_node.h
+++ b/include/adc_jury_server_alg_node.h
@@ -38,8 +38,6 @@
#include <iri_action_server/iri_action_server.h>
#include <iri_adc_msgs/adc_juryAction.h>
-#include <sstream>
-
/**
* \brief IRI ROS Specific Algorithm Class
*
@@ -76,7 +74,8 @@ class AdcJuryServerAlgNode : public algorithm_base::IriBaseAlgorithm<AdcJuryServ
// [action client attributes]
- std::stringstream feedback_msg;
+ std::string feedback_msg;
+ iri_adc_msgs::adc_goal_array goal_array;
/**
* \brief config variable
*
diff --git a/src/adc_jury_server_alg_node.cpp b/src/adc_jury_server_alg_node.cpp
index d9d0ca072ce3d7ffccef5ff2ba522307bcceeb33..1b4b42fa8c34a52b9dae6bc8d8f6d0fd6eaac6f2 100644
--- a/src/adc_jury_server_alg_node.cpp
+++ b/src/adc_jury_server_alg_node.cpp
@@ -50,6 +50,15 @@ void AdcJuryServerAlgNode::mainNodeThread(void)
//lock access to algorithm if necessary
this->alg_.lock();
ROS_DEBUG("AdcJuryServerAlgNode::mainNodeThread");
+
+ if(this->adc_jury_active)
+ {
+ //Example filling feedback status string, assigned in feedback callback
+ this->feedback_msg = iri_adc_msgs::adc_juryFeedback::NAVIGATING;
+ if(this->goal_array.goals.size()>0)
+ this->feedback_msg += " " + this->goal_array.goals[0].id;
+ }
+
// [fill msg structures]
// [fill srv structure and make request to the server]
@@ -84,11 +93,6 @@ 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)
@@ -120,6 +124,7 @@ void AdcJuryServerAlgNode::adc_juryStartCallback(const iri_adc_msgs::adc_juryGoa
this->alg_.lock();
ROS_INFO("AdcJuryServerAlgNode::adc_juryStartCallback: action started!");
//check goal
+ this->goal_array = goal->goals;
this->adc_jury_active=true;
this->adc_jury_succeeded=false;
this->adc_jury_finished=false;
@@ -172,12 +177,8 @@ void AdcJuryServerAlgNode::adc_juryGetResultCallback(iri_adc_msgs::adc_juryResul
void AdcJuryServerAlgNode::adc_juryGetFeedbackCallback(iri_adc_msgs::adc_juryFeedbackPtr& feedback)
{
this->alg_.lock();
- static int i=0;
//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("");
+ feedback->status = this->feedback_msg;
//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)
@@ -209,56 +210,48 @@ 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)
+
+ if(config.car_lights!= this->config_.car_lights)
{
- set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_CHARGING;
- this->feedback_msg << "set car lights in mode CHARGING" << std::endl;
+ if(config.car_lights==0)
+ {
+ set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_OFF;
+ }
+ else if(config.car_lights==1)
+ {
+ set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_NORMAL_DRIVE;
+ }
+ else if(config.car_lights==2)
+ {
+ set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_BRAKE;
+ }
+ else if(config.car_lights==3)
+ {
+ set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_TURN_LEFT;
+ }
+ else if(config.car_lights==4)
+ {
+ set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_TURN_RIGHT;
+ }
+ else if(config.car_lights==5)
+ {
+ set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_PARKED;
+ }
+ else if(config.car_lights==6)
+ {
+ set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_EMERGENCY;
+ }
+ else if(config.car_lights==7)
+ {
+ set_lights_srv_.request.lights=iri_adc_msgs::set_car_lightsRequest::CAR_AMBULANCE;
+ }
+
+ 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_ERROR("AdcJuryServerAlgNode:: Failed to call service on topic %s",this->set_lights_client_.getService().c_str());
}
- 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();