From 15b36f1e91e1f501b0b6b1aca1a973b903d2825e Mon Sep 17 00:00:00 2001
From: shernand <shernand@iri.upc.edu>
Date: Mon, 25 Oct 2021 20:11:13 +0200
Subject: [PATCH] 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 | 2 +
cfg/AdcJuryServer.cfg | 13 ++++++
include/adc_jury_server_alg_node.h | 8 +++-
src/adc_jury_server_alg_node.cpp | 67 +++++++++++++++++++++++++++---
4 files changed, 83 insertions(+), 7 deletions(-)
diff --git a/README.md b/README.md
index 0c0a47b..2cb59d2 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 9c8ea6a..fe4edc4 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 0310282..da29f8c 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 f739613..d9d0ca0 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();
}
--
GitLab