Skip to content
Snippets Groups Projects
Commit 4b650362 authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Switch feedback to string. Show sample filling feedback string with msg... 

Switch feedback to string. Show sample filling feedback string with msg constant and goal id. Call lights service if cfg has changed
parent 15b36f1e
No related branches found
No related tags found
1 merge request!1Added the service to control the car lights.
Hide whitespace changes
Inline Side-by-side
cfg/AdcJuryServer.cfg
+ 1
− 2
View file @ 4b650362
......@@ -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)
......
include/adc_jury_server_alg_node.h
+ 2
− 3
View file @ 4b650362
......@@ -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
*
......
src/adc_jury_server_alg_node.cpp
+ 51
− 58
View file @ 4b650362
......@@ -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();
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment