diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2c692f89454030e842fd900a9f2758a3e5584ba2..ea1130b740386d946a08c6e0bb8a905912022f96 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_driver)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver iri_adc_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -64,7 +64,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_driver
+ CATKIN_DEPENDS iri_base_driver iri_adc_msgs
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -106,6 +106,7 @@ target_link_libraries(${PROJECT_NAME} ${rgb_leds_driver_LIBRARIES})
# Add message headers dependencies
# ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} iri_adc_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} iri_base_driver_generate_messages_cpp)
add_dependencies(${PROJECT_NAME}_gencfg iri_base_driver_generate_messages_py)
# ********************************************************************
diff --git a/README.md b/README.md
index 516ac5b9aab1c143c4517c3af26dc013c02a1bf2..c5bd129b64dd50d58559b651a9b39bb154a67010 100644
--- a/README.md
+++ b/README.md
@@ -3,6 +3,8 @@
The iri_adc_car_lights project description
# ROS Interface
+### Service servers
+ - ~**set_lights** (iri_adc_msgs/set_car_lights.srv)
### Parameters
- ~**rate** (Double; default: 10.0; min: 0.1; max: 1000) The main node thread loop rate in Hz.
diff --git a/cfg/AdcCarLights.cfg b/cfg/AdcCarLights.cfg
index 54d4e6785c708010ab5fc8777402e8ca248b3c23..f5a4fe9ea79a7300065f43feda98dcd6873164ba 100755
--- a/cfg/AdcCarLights.cfg
+++ b/cfg/AdcCarLights.cfg
@@ -37,82 +37,11 @@ from iri_base_driver.msg import SensorLevels
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
-standard = gen.add_group("standard")
-brake = gen.add_group("brake")
-turn = gen.add_group("turn")
-parked = gen.add_group("parked")
-emergency = gen.add_group("emergency")
-health = gen.add_group("health")
-charge = gen.add_group("charge")
# Name Type Reconf.level Description standard Min Max
gen.add("rate", double_t, SensorLevels.RECONFIGURE_STOP, "Main loop rate (Hz)", 10.0, 0.1, 1000.0)
gen.add("serial_port", str_t, SensorLevels.RECONFIGURE_STOP, "Serial port Linux device", "/dev/ttyACM0")
gen.add("baudrate", int_t, SensorLevels.RECONFIGURE_STOP, "Serial port baudrate", 56700, 9600, 1000000)
gen.add("device_id", int_t, SensorLevels.RECONFIGURE_STOP, "car_lights device ID", 1, 1, 254)
-gen.add("num_rows", int_t, SensorLevels.RECONFIGURE_STOP, "Number of rows of the LED array", 6, 1, 200)
-gen.add("num_cols", int_t, SensorLevels.RECONFIGURE_STOP, "Number of columns of the LED array",8, 1, 200)
-gen.add("front_left_start_row",int_t,SensorLevels.RECONFIGURE_STOP, "Start row of the front left lights",0, 0, 200)
-gen.add("front_left_num_rows",int_t,SensorLevels.RECONFIGURE_STOP, "Start row of the front left lights",1, 1, 200)
-gen.add("front_left_start_col",int_t,SensorLevels.RECONFIGURE_STOP, "Start column of the front left lights",0, 0, 200)
-gen.add("front_left_num_cols",int_t,SensorLevels.RECONFIGURE_STOP, "Start column of the front left lights",8, 1, 200)
-gen.add("front_right_start_row",int_t,SensorLevels.RECONFIGURE_STOP, "Start row of the front right lights",1, 0, 200)
-gen.add("front_right_num_rows",int_t,SensorLevels.RECONFIGURE_STOP, "Start row of the front right lights",1, 1, 200)
-gen.add("front_right_start_col",int_t,SensorLevels.RECONFIGURE_STOP, "Start column of the front right lights",0, 0, 200)
-gen.add("front_right_num_cols",int_t,SensorLevels.RECONFIGURE_STOP, "Start column of the front right lights",8, 1, 200)
-gen.add("rear_left_start_row",int_t,SensorLevels.RECONFIGURE_STOP, "Start row of the rear left lights",4, 0, 200)
-gen.add("rear_left_num_rows",int_t,SensorLevels.RECONFIGURE_STOP, "Start row of the rear left lights",1, 1, 200)
-gen.add("rear_left_start_col",int_t,SensorLevels.RECONFIGURE_STOP, "Start column of the rear left lights",0, 0, 200)
-gen.add("rear_left_num_cols",int_t,SensorLevels.RECONFIGURE_STOP, "Start column of the rear left lights",8, 1, 200)
-gen.add("rear_right_start_row",int_t,SensorLevels.RECONFIGURE_STOP, "Start row of the rear right lights",5, 0, 200)
-gen.add("rear_right_num_rows",int_t,SensorLevels.RECONFIGURE_STOP, "Start row of the rear right lights",1, 1, 200)
-gen.add("rear_right_start_col",int_t,SensorLevels.RECONFIGURE_STOP, "Start column of the rear right lights",0, 0, 200)
-gen.add("rear_right_num_cols",int_t,SensorLevels.RECONFIGURE_STOP, "Start column of the rear right lights",8, 1, 200)
-gen.add("clear_all", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Turn off the led array", False)
-standard.add("standard_front_R", int_t,SensorLevels.RECONFIGURE_RUNNING, "RED component for the front standard configuration", 32, 0, 255)
-standard.add("standard_front_G", int_t,SensorLevels.RECONFIGURE_RUNNING, "GREEN component for the front standard configuration", 32, 0, 255)
-standard.add("standard_front_B", int_t,SensorLevels.RECONFIGURE_RUNNING, "BLUE component for the front standard configuration", 32, 0, 255)
-standard.add("standard_rear_R", int_t,SensorLevels.RECONFIGURE_RUNNING, "RED component for the rear standard configuration", 32, 0, 255)
-standard.add("standard_rear_G", int_t,SensorLevels.RECONFIGURE_RUNNING, "GREEN component for the rear standard configuration", 0, 0, 255)
-standard.add("standard_rear_B", int_t,SensorLevels.RECONFIGURE_RUNNING, "BLUE component for the rear standard configuration", 0, 0, 255)
-standard.add("standard_active",bool_t,SensorLevels.RECONFIGURE_RUNNING, "Set the standard lights configuration", False)
-
-brake.add("brake_rear_R", int_t,SensorLevels.RECONFIGURE_RUNNING, "RED component for the rear brake configuration", 64, 0, 255)
-brake.add("brake_rear_G", int_t,SensorLevels.RECONFIGURE_RUNNING, "GREEN component for the rear brake configuration", 0, 0, 255)
-brake.add("brake_rear_B", int_t,SensorLevels.RECONFIGURE_RUNNING, "BLUE component for the rear brake configuration", 0, 0, 255)
-brake.add("brake_active",bool_t,SensorLevels.RECONFIGURE_RUNNING, "Set the brake lights configuration", False)
-
-turn.add("turn_R", int_t, SensorLevels.RECONFIGURE_RUNNING, "RED component for the turn configuration", 32, 0, 255)
-turn.add("turn_G", int_t, SensorLevels.RECONFIGURE_RUNNING, "GREEN component for the turn configuration", 8, 0, 255)
-turn.add("turn_B", int_t, SensorLevels.RECONFIGURE_RUNNING, "BLUE component for the turn configuration", 0, 0, 255)
-turn.add("turn_period",double_t,SensorLevels.RECONFIGURE_RUNNING, "turn configuration blinking period", 1.0, 0.5, 5.0)
-turn.add("turn_left_active",bool_t, SensorLevels.RECONFIGURE_RUNNING, "Set the turn left configuration", False)
-turn.add("turn_right_active",bool_t, SensorLevels.RECONFIGURE_RUNNING, "Set the turn right configuration", False)
-
-parked.add("parked_R", int_t,SensorLevels.RECONFIGURE_RUNNING, "RED component for the parked configuration", 0, 0, 255)
-parked.add("parked_G", int_t,SensorLevels.RECONFIGURE_RUNNING, "GREEN component for the parked configuration", 0, 0, 255)
-parked.add("parked_B", int_t,SensorLevels.RECONFIGURE_RUNNING, "BLUE component for the parked configuration", 32, 0, 255)
-parked.add("parked_active",bool_t,SensorLevels.RECONFIGURE_RUNNING, "Set the parked lights configuration", False)
-
-emergency.add("emergency_R", int_t, SensorLevels.RECONFIGURE_RUNNING, "RED component for the emergency configuration", 32, 0, 255)
-emergency.add("emergency_G", int_t, SensorLevels.RECONFIGURE_RUNNING, "GREEN component for the emergency configuration", 8, 0, 255)
-emergency.add("emergency_B", int_t, SensorLevels.RECONFIGURE_RUNNING, "BLUE component for the emergency configuration", 0, 0, 255)
-emergency.add("emergency_period",double_t,SensorLevels.RECONFIGURE_RUNNING, "emergency configuration blinking period", 1.0, 0.5, 5.0)
-emergency.add("emergency_active",bool_t, SensorLevels.RECONFIGURE_RUNNING, "Set the emergency configuration", False)
-
-health.add("health_color1_R", int_t,SensorLevels.RECONFIGURE_RUNNING, "RED component for the health configuration", 0, 0, 255)
-health.add("health_color1_G", int_t,SensorLevels.RECONFIGURE_RUNNING, "GREEN component for the health configuration", 0, 0, 255)
-health.add("health_color1_B", int_t,SensorLevels.RECONFIGURE_RUNNING, "BLUE component for the health configuration", 32, 0, 255)
-health.add("health_color2_R", int_t,SensorLevels.RECONFIGURE_RUNNING, "RED component for the health configuration", 32, 0, 255)
-health.add("health_color2_G", int_t,SensorLevels.RECONFIGURE_RUNNING, "GREEN component for the health configuration", 0, 0, 255)
-health.add("health_color2_B", int_t,SensorLevels.RECONFIGURE_RUNNING, "BLUE component for the health configuration", 0, 0, 255)
-health.add("health_period",double_t,SensorLevels.RECONFIGURE_RUNNING, "health configuration blinking period", 1.0, 0.5, 5.0)
-health.add("health_active",bool_t,SensorLevels.RECONFIGURE_RUNNING, "Set the health lights configuration", False)
-
-charge.add("charge_R", int_t, SensorLevels.RECONFIGURE_RUNNING, "RED component for the charge configuration", 0, 0, 255)
-charge.add("charge_G", int_t, SensorLevels.RECONFIGURE_RUNNING, "GREEN component for the charge configuration", 0, 0, 255)
-charge.add("charge_B", int_t, SensorLevels.RECONFIGURE_RUNNING, "BLUE component for the charge configuration", 64, 0, 255)
-charge.add("charge_period",double_t,SensorLevels.RECONFIGURE_RUNNING, "charge configuration blinking period", 1.0, 0.5, 5.0)
-charge.add("charge_active",bool_t, SensorLevels.RECONFIGURE_RUNNING, "Set the charge configuration", False)
exit(gen.generate(PACKAGE, "AdcCarLightsDriver", "AdcCarLights"))
diff --git a/config/params.yaml b/config/params.yaml
index 564c35f000245eac588324ad6892380109f762d0..0f554c1fa8c06ec6b2ee5ee2faff15b5cb39fe2e 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -2,69 +2,4 @@ rate: 10
serial_port: "/dev/ttyACM0"
baudrate: 57600
device_id: 1
-num_rows: 6
-num_cols: 8
-front_left_start_row: 0
-front_left_num_rows: 1
-front_left_start_col: 0
-front_left_num_cols: 8
-front_right_start_row: 1
-front_right_num_rows: 1
-front_right_start_col: 0
-front_right_num_cols: 8
-rear_left_start_row: 4
-rear_left_num_rows: 1
-rear_left_start_col: 0
-rear_left_num_cols: 8
-rear_right_start_row: 5
-rear_right_num_rows: 1
-rear_right_start_col: 0
-rear_right_num_cols: 8
-clear_all: False
-
-standard_front_R: 32
-standard_front_G: 32
-standard_front_B: 32
-standard_rear_R: 32
-standard_rear_G: 0
-standard_rear_B: 0
-standard_active: False
-
-brake_rear_R: 64
-brake_rear_G: 0
-brake_rear_B: 0
-brake_active: False
-
-turn_R: 32
-turn_G: 8
-turn_B: 0
-turn_period: 1.0
-turn_left_active: False
-turn_right_active: False
-
-parked_R: 0
-parked_G: 0
-parked_B: 32
-parked_active: False
-
-emergency_R: 32
-emergency_G: 8
-emergency_B: 0
-emergency_period: 1.0
-emergency_active: False
-
-health_color1_R: 0
-health_color1_G: 0
-health_color1_B: 32
-health_color2_R: 32
-health_color2_G: 0
-health_color2_B: 0
-health_period: 1.0
-health_active: False
-
-charge_R: 0
-charge_G: 0
-charge_B: 64
-charge_period: 1.0
-charge_active: False
diff --git a/include/adc_car_lights_driver.h b/include/adc_car_lights_driver.h
index 7add31ec4d6ba3637142c3c7b865855e22b28357..e33cb3abf3137d0671792727d9880f8f80f07b79 100644
--- a/include/adc_car_lights_driver.h
+++ b/include/adc_car_lights_driver.h
@@ -57,6 +57,16 @@ class AdcCarLightsDriver : public iri_base_driver::IriBaseDriver
// private attributes and methods
CRGBLedsFrameBufferDriver *frame_buffer;
CRGBLedsDriver *leds;
+ TLEDArea whole_front_area;
+ TLEDArea front_right_area;
+ TLEDArea front_left_area;
+ TLEDArea whole_rear_area;
+ TLEDArea rear_left_area;
+ TLEDArea rear_middle_left_area;
+ TLEDArea rear_center_area;
+ TLEDArea rear_middle_right_area;
+ TLEDArea rear_right_area;
+ TLEDArea whole_area;
public:
/**
* \brief define config type
diff --git a/include/adc_car_lights_driver_node.h b/include/adc_car_lights_driver_node.h
index 5809247b78a17c36498c9aa5da18d08d4949985d..cfe1be9d4a4b1ea1711ad1ef1d1f6459ff6813dd 100644
--- a/include/adc_car_lights_driver_node.h
+++ b/include/adc_car_lights_driver_node.h
@@ -31,6 +31,7 @@
// [publisher subscriber headers]
// [service client headers]
+#include <iri_adc_msgs/set_car_lights.h>
// [action server client headers]
@@ -59,6 +60,12 @@ class AdcCarLightsDriverNode : public iri_base_driver::IriBaseNodeDriver<AdcCarL
// [subscriber attributes]
// [service attributes]
+ ros::ServiceServer set_lights_server_;
+ bool set_lightsCallback(iri_adc_msgs::set_car_lights::Request &req, iri_adc_msgs::set_car_lights::Response &res);
+ pthread_mutex_t set_lights_mutex_;
+ void set_lights_mutex_enter(void);
+ void set_lights_mutex_exit(void);
+
// [client attributes]
diff --git a/launch/node.launch b/launch/node.launch
index 617b26d34b853287def2e5100a088216268de99c..6f27a49f833db4784b912f29f9739fc687e79c06 100644
--- a/launch/node.launch
+++ b/launch/node.launch
@@ -1,11 +1,11 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
- <arg name="node_name" default="iri_adc_car_lights"/>
- <arg name="output" default="screen"/>
- <arg name="launch_prefix" default=""/>
- <arg name="config_file" default="$(find iri_adc_car_lights)/config/params.yaml"/>
- <!-- <arg name="topic_name" default="new_topic_name"/> -->
+ <arg name="node_name" default="iri_adc_car_lights"/>
+ <arg name="output" default="screen"/>
+ <arg name="launch_prefix" default=""/>
+ <arg name="config_file" default="$(find iri_adc_car_lights)/config/params.yaml"/>
+ <arg name="set_lights_service" default="set_lights"/>
<node name="$(arg node_name)"
pkg ="iri_adc_car_lights"
@@ -13,7 +13,7 @@
output="$(arg output)"
launch-prefix="$(arg launch_prefix)">
<rosparam file="$(arg config_file)" command="load"/>
- <!--<remap from="~/topic" to="$(arg topic_name)"/>-->
+ <remap from="~/set_lights" to="$(arg set_lights_service)"/>
</node>
</launch>
diff --git a/launch/test.launch b/launch/test.launch
index 8acc915ef261aaa326b7c44c8ca3e4600eb615f9..63f134511a0ea22ea6f2927b4c3fcdb9b17d0e76 100644
--- a/launch/test.launch
+++ b/launch/test.launch
@@ -6,9 +6,11 @@
<arg name="dr" default="true"/>
<include file="$(find iri_adc_car_lights)/launch/node.launch">
- <arg name="node_name" value="iri_adc_car_lights"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <arg name="node_name" value="iri_adc_car_lights"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <arg name="config_file" value="$(find iri_adc_car_lights)/config/params.yaml"/>
+ <arg name="set_lights_service" value="set_lights"/>
</include>
<node name="rqt_reconfigure_iri_adc_car_lights"
diff --git a/package.xml b/package.xml
index 56a95d4d10aa0eec8271da98282de9f73c05ef33..103adafdc66db58d672af3018450d4eb6ccb6c6b 100644
--- a/package.xml
+++ b/package.xml
@@ -52,6 +52,7 @@
<build_depend>iri_base_driver</build_depend>
<build_export_depend>iri_base_driver</build_export_depend>
<exec_depend>iri_base_driver</exec_depend>
+ <depend>iri_adc_msgs</depend>
<!-- The export tag contains other, unspecified, tags -->
diff --git a/src/adc_car_lights_driver.cpp b/src/adc_car_lights_driver.cpp
index 95398cb104693cb01158b67ae5a40eb4f8f773c0..a45e6c8fd2cc682ab5b6c87de6b97b7949b985ac 100644
--- a/src/adc_car_lights_driver.cpp
+++ b/src/adc_car_lights_driver.cpp
@@ -10,22 +10,112 @@ AdcCarLightsDriver::AdcCarLightsDriver(void)
bool AdcCarLightsDriver::openDriver(void)
{
- TLEDArea area;
-
//setDriverId(driver string id);
try{
this->frame_buffer=new CRGBLedsFrameBufferDriver("frame_buffer",this->config_.serial_port,this->config_.baudrate,this->config_.device_id);
this->leds=new CRGBLedsDriver("leds_cont",this->config_.serial_port,this->config_.baudrate,this->config_.device_id);
// configure the array
- this->frame_buffer->set_num_rows(this->config_.num_rows);
- this->frame_buffer->set_num_pixels_per_row(this->config_.num_cols);
+ this->frame_buffer->set_num_rows(1);
+// this->frame_buffer->set_num_pixels_per_row(24);
+ this->frame_buffer->set_num_pixels_per_row(48);
if((signed int)this->frame_buffer->get_free_memory()<0)
throw CException(_HERE_,"Not enough memory, reduce number of rows and/or columns");
this->frame_buffer->clear_all_patterns();
this->leds->set_frame_buffer_info(this->frame_buffer->get_num_rows(),this->frame_buffer->get_num_pixels_per_row(),this->frame_buffer->get_num_buffers());
- this->leds->get_active_area(area);
- this->leds->set_active_area(area);
-
+ this->leds->get_active_area(this->whole_area);
+ this->leds->set_active_area(this->whole_area);
+ // whole front area
+ this->whole_front_area.min_row=0;
+ this->whole_front_area.max_row=0;
+ this->whole_front_area.min_col=0;
+ this->whole_front_area.max_col=15;
+ // front right area
+ this->front_right_area.min_row=0;
+ this->front_right_area.max_row=0;
+ this->front_right_area.min_col=0;
+ this->front_right_area.max_col=7;
+ // front left area
+ this->front_left_area.min_row=0;
+ this->front_left_area.max_row=0;
+ this->front_left_area.min_col=8;
+ this->front_left_area.max_col=15;
+ // rear left area
+ this->rear_left_area.min_row=0;
+ this->rear_left_area.max_row=0;
+ this->rear_left_area.min_col=16;
+ this->rear_left_area.max_col=23;
+ // rear center area
+ this->rear_center_area.min_row=0;
+ this->rear_center_area.max_row=0;
+ this->rear_center_area.min_col=24;
+ this->rear_center_area.max_col=39;
+ // rear middle left area
+ this->rear_middle_left_area.min_row=0;
+ this->rear_middle_left_area.max_row=0;
+ this->rear_middle_left_area.min_col=24;
+ this->rear_middle_left_area.max_col=31;
+ // rear middle right area
+ this->rear_middle_right_area.min_row=0;
+ this->rear_middle_right_area.max_row=0;
+ this->rear_middle_right_area.min_col=32;
+ this->rear_middle_right_area.max_col=39;
+ // rear right area
+ this->rear_right_area.min_row=0;
+ this->rear_right_area.max_row=0;
+ this->rear_right_area.min_col=40;
+ this->rear_right_area.max_col=47;
+ // whole rear area
+ this->whole_rear_area.min_row=0;
+ this->whole_rear_area.max_row=0;
+ this->whole_rear_area.min_col=16;
+ this->whole_rear_area.max_col=47;
+/*
+ // whole front area
+ this->whole_front_area.min_row=0;
+ this->whole_front_area.max_row=0;
+ this->whole_front_area.min_col=0;
+ this->whole_front_area.max_col=7;
+ // front right area
+ this->front_right_area.min_row=0;
+ this->front_right_area.max_row=0;
+ this->front_right_area.min_col=0;
+ this->front_right_area.max_col=3;
+ // front left area
+ this->front_left_area.min_row=0;
+ this->front_left_area.max_row=0;
+ this->front_left_area.min_col=4;
+ this->front_left_area.max_col=7;
+ // rear left area
+ this->rear_left_area.min_row=0;
+ this->rear_left_area.max_row=0;
+ this->rear_left_area.min_col=8;
+ this->rear_left_area.max_col=11;
+ // rear center area
+ this->rear_center_area.min_row=0;
+ this->rear_center_area.max_row=0;
+ this->rear_center_area.min_col=8;
+ this->rear_center_area.max_col=11;
+ // rear middle left area
+ this->rear_middle_left_area.min_row=0;
+ this->rear_middle_left_area.max_row=0;
+ this->rear_middle_left_area.min_col=12;
+ this->rear_middle_left_area.max_col=19;
+ // rear middle right area
+ this->rear_middle_right_area.min_row=0;
+ this->rear_middle_right_area.max_row=0;
+ this->rear_middle_right_area.min_col=16;
+ this->rear_middle_right_area.max_col=19;
+ // rear right area
+ this->rear_right_area.min_row=0;
+ this->rear_right_area.max_row=0;
+ this->rear_right_area.min_col=20;
+ this->rear_right_area.max_col=23;
+ // whole rear area
+ this->whole_rear_area.min_row=0;
+ this->whole_rear_area.max_row=0;
+ this->whole_rear_area.min_col=8;
+ this->whole_rear_area.max_col=23;
+*/
return true;
}catch(CException &e){
ROS_WARN_STREAM(e.what());
@@ -73,6 +163,7 @@ bool AdcCarLightsDriver::startDriver(void)
bool AdcCarLightsDriver::stopDriver(void)
{
try{
+ this->clear_all();
this->leds->stop();
this->frame_buffer->stop();
}catch(CException &e){
@@ -108,7 +199,8 @@ void AdcCarLightsDriver::config_update(Config& new_cfg, uint32_t level)
void AdcCarLightsDriver::clear_all(void)
{
try{
- this->frame_buffer->clear_all_patterns();
+ if(this->frame_buffer!=NULL)
+ this->frame_buffer->clear_all_patterns();
}catch(CException &e){
this->goState(iri_base_driver::CLOSED);
}
@@ -117,43 +209,22 @@ void AdcCarLightsDriver::clear_all(void)
void AdcCarLightsDriver::set_standard(void)
{
TPixelRGB color;
- TLEDArea area;
try{
- this->frame_buffer->clear_all_patterns();
- color.R=this->config_.standard_front_R;
- color.G=this->config_.standard_front_G;
- color.B=this->config_.standard_front_B;
- area.min_row=this->config_.front_left_start_row;
- area.max_row=this->config_.front_left_start_row+this->config_.front_left_num_rows-1;
- area.min_col=this->config_.front_left_start_col;
- area.max_col=this->config_.front_left_start_col+this->config_.front_left_num_cols-1;
- this->frame_buffer->set_color(color,area);
- color.R=this->config_.standard_front_R;
- color.G=this->config_.standard_front_G;
- color.B=this->config_.standard_front_B;
- area.min_row=this->config_.front_right_start_row;
- area.max_row=this->config_.front_right_start_row+this->config_.front_right_num_rows-1;
- area.min_col=this->config_.front_right_start_col;
- area.max_col=this->config_.front_right_start_col+this->config_.front_right_num_cols-1;
- this->frame_buffer->set_color(color,area);
- color.R=this->config_.standard_rear_R;
- color.G=this->config_.standard_rear_G;
- color.B=this->config_.standard_rear_B;
- area.min_row=this->config_.rear_left_start_row;
- area.max_row=this->config_.rear_left_start_row+this->config_.rear_left_num_rows-1;
- area.min_col=this->config_.rear_left_start_col;
- area.max_col=this->config_.rear_left_start_col+this->config_.rear_left_num_cols-1;
- this->frame_buffer->set_color(color,area);
- color.R=this->config_.standard_rear_R;
- color.G=this->config_.standard_rear_G;
- color.B=this->config_.standard_rear_B;
- area.min_row=this->config_.rear_right_start_row;
- area.max_row=this->config_.rear_right_start_row+this->config_.rear_right_num_rows-1;
- area.min_col=this->config_.rear_right_start_col;
- area.max_col=this->config_.rear_right_start_col+this->config_.rear_right_num_cols-1;
- this->frame_buffer->set_color(color,area);
+ if(this->frame_buffer!=NULL)
+ {
+ this->frame_buffer->clear_all_patterns();
+ color.R=255;
+ color.G=255;
+ color.B=255;
+ this->frame_buffer->set_color(color,this->whole_front_area);
+ color.R=128;
+ color.G=0;
+ color.B=0;
+ this->frame_buffer->set_color(color,this->whole_rear_area);
+ }
}catch(CException &e){
+ ROS_WARN_STREAM(e.what());
this->goState(iri_base_driver::CLOSED);
}
}
@@ -161,43 +232,30 @@ void AdcCarLightsDriver::set_standard(void)
void AdcCarLightsDriver::set_brake(void)
{
TPixelRGB color;
- TLEDArea area;
try{
- this->frame_buffer->clear_all_patterns();
- color.R=this->config_.standard_front_R;
- color.G=this->config_.standard_front_G;
- color.B=this->config_.standard_front_B;
- area.min_row=this->config_.front_left_start_row;
- area.max_row=this->config_.front_left_start_row+this->config_.front_left_num_rows-1;
- area.min_col=this->config_.front_left_start_col;
- area.max_col=this->config_.front_left_start_col+this->config_.front_left_num_cols-1;
- this->frame_buffer->set_color(color,area);
- color.R=this->config_.standard_front_R;
- color.G=this->config_.standard_front_G;
- color.B=this->config_.standard_front_B;
- area.min_row=this->config_.front_right_start_row;
- area.max_row=this->config_.front_right_start_row+this->config_.front_right_num_rows-1;
- area.min_col=this->config_.front_right_start_col;
- area.max_col=this->config_.front_right_start_col+this->config_.front_right_num_cols-1;
- this->frame_buffer->set_color(color,area);
- color.R=this->config_.brake_rear_R;
- color.G=this->config_.brake_rear_G;
- color.B=this->config_.brake_rear_B;
- area.min_row=this->config_.rear_left_start_row;
- area.max_row=this->config_.rear_left_start_row+this->config_.rear_left_num_rows-1;
- area.min_col=this->config_.rear_left_start_col;
- area.max_col=this->config_.rear_left_start_col+this->config_.rear_left_num_cols-1;
- this->frame_buffer->set_color(color,area);
- color.R=this->config_.brake_rear_R;
- color.G=this->config_.brake_rear_G;
- color.B=this->config_.brake_rear_B;
- area.min_row=this->config_.rear_right_start_row;
- area.max_row=this->config_.rear_right_start_row+this->config_.rear_right_num_rows-1;
- area.min_col=this->config_.rear_right_start_col;
- area.max_col=this->config_.rear_right_start_col+this->config_.rear_right_num_cols-1;
- this->frame_buffer->set_color(color,area);
+ if(this->frame_buffer!=NULL)
+ {
+ this->frame_buffer->clear_all_patterns();
+ color.R=255;
+ color.G=255;
+ color.B=255;
+ this->frame_buffer->set_color(color,this->whole_front_area);
+ color.R=255;
+ color.G=0;
+ color.B=0;
+ this->frame_buffer->set_color(color,this->rear_left_area);
+ color.R=255;
+ color.G=0;
+ color.B=0;
+ this->frame_buffer->set_color(color,this->rear_right_area);
+ color.R=128;
+ color.G=0;
+ color.B=0;
+ this->frame_buffer->set_color(color,this->rear_center_area);
+ }
}catch(CException &e){
+ ROS_WARN_STREAM(e.what());
this->goState(iri_base_driver::CLOSED);
}
}
@@ -205,49 +263,31 @@ void AdcCarLightsDriver::set_brake(void)
void AdcCarLightsDriver::set_turn_left(void)
{
TPixelRGB max,min,color;
- TLEDArea area;
try{
- this->frame_buffer->clear_all_patterns();
- max.R=this->config_.turn_R;
- max.G=this->config_.turn_G;
- max.B=this->config_.turn_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.front_left_start_row;
- area.max_row=this->config_.front_left_start_row+this->config_.front_left_num_rows-1;
- area.min_col=this->config_.front_left_start_col;
- area.max_col=this->config_.front_left_start_col+this->config_.front_left_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.turn_period,this->config_.turn_period/2.0,max,min,area);
- color.R=this->config_.standard_front_R;
- color.G=this->config_.standard_front_G;
- color.B=this->config_.standard_front_B;
- area.min_row=this->config_.front_right_start_row;
- area.max_row=this->config_.front_right_start_row+this->config_.front_right_num_rows-1;
- area.min_col=this->config_.front_right_start_col;
- area.max_col=this->config_.front_right_start_col+this->config_.front_right_num_cols-1;
- this->frame_buffer->set_color(color,area);
- max.R=this->config_.turn_R;
- max.G=this->config_.turn_G;
- max.B=this->config_.turn_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.rear_left_start_row;
- area.max_row=this->config_.rear_left_start_row+this->config_.rear_left_num_rows-1;
- area.min_col=this->config_.rear_left_start_col;
- area.max_col=this->config_.rear_left_start_col+this->config_.rear_left_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.turn_period,this->config_.turn_period/2.0,max,min,area);
- color.R=this->config_.brake_rear_R;
- color.G=this->config_.brake_rear_G;
- color.B=this->config_.brake_rear_B;
- area.min_row=this->config_.rear_right_start_row;
- area.max_row=this->config_.rear_right_start_row+this->config_.rear_right_num_rows-1;
- area.min_col=this->config_.rear_right_start_col;
- area.max_col=this->config_.rear_right_start_col+this->config_.rear_right_num_cols-1;
- this->frame_buffer->set_color(color,area);
+ if(this->frame_buffer!=NULL)
+ {
+ this->frame_buffer->clear_all_patterns();
+ color.R=255;
+ color.G=255;
+ color.B=255;
+ this->frame_buffer->set_color(color,this->front_right_area);
+ max.R=255;
+ max.G=64;
+ max.B=0;
+ min.R=0;
+ min.G=0;
+ min.B=0;
+ this->frame_buffer->add_square_waveform(1.0,0.5,max,min,this->front_left_area);
+ this->frame_buffer->add_square_waveform(1.0,0.5,max,min,this->rear_left_area);
+ color.R=128;
+ color.G=0;
+ color.B=0;
+ this->frame_buffer->set_color(color,this->rear_center_area);
+ this->frame_buffer->set_color(color,this->rear_right_area);
+ }
}catch(CException &e){
+ ROS_WARN_STREAM(e.what());
this->goState(iri_base_driver::CLOSED);
}
}
@@ -255,49 +295,31 @@ void AdcCarLightsDriver::set_turn_left(void)
void AdcCarLightsDriver::set_turn_right(void)
{
TPixelRGB max,min,color;
- TLEDArea area;
try{
- this->frame_buffer->clear_all_patterns();
- color.R=this->config_.standard_front_R;
- color.G=this->config_.standard_front_G;
- color.B=this->config_.standard_front_B;
- area.min_row=this->config_.front_left_start_row;
- area.max_row=this->config_.front_left_start_row+this->config_.front_left_num_rows-1;
- area.min_col=this->config_.front_left_start_col;
- area.max_col=this->config_.front_left_start_col+this->config_.front_left_num_cols-1;
- this->frame_buffer->set_color(color,area);
- max.R=this->config_.turn_R;
- max.G=this->config_.turn_G;
- max.B=this->config_.turn_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.front_right_start_row;
- area.max_row=this->config_.front_right_start_row+this->config_.front_right_num_rows-1;
- area.min_col=this->config_.front_right_start_col;
- area.max_col=this->config_.front_right_start_col+this->config_.front_right_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.turn_period,this->config_.turn_period/2.0,max,min,area);
- color.R=this->config_.brake_rear_R;
- color.G=this->config_.brake_rear_G;
- color.B=this->config_.brake_rear_B;
- area.min_row=this->config_.rear_left_start_row;
- area.max_row=this->config_.rear_left_start_row+this->config_.rear_left_num_rows-1;
- area.min_col=this->config_.rear_left_start_col;
- area.max_col=this->config_.rear_left_start_col+this->config_.rear_left_num_cols-1;
- this->frame_buffer->set_color(color,area);
- max.R=this->config_.turn_R;
- max.G=this->config_.turn_G;
- max.B=this->config_.turn_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.rear_right_start_row;
- area.max_row=this->config_.rear_right_start_row+this->config_.rear_right_num_rows-1;
- area.min_col=this->config_.rear_right_start_col;
- area.max_col=this->config_.rear_right_start_col+this->config_.rear_right_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.turn_period,this->config_.turn_period/2.0,max,min,area);
+ if(this->frame_buffer!=NULL)
+ {
+ this->frame_buffer->clear_all_patterns();
+ color.R=255;
+ color.G=255;
+ color.B=255;
+ this->frame_buffer->set_color(color,this->front_left_area);
+ max.R=255;
+ max.G=64;
+ max.B=0;
+ min.R=0;
+ min.G=0;
+ min.B=0;
+ this->frame_buffer->add_square_waveform(1.0,0.5,max,min,this->front_right_area);
+ this->frame_buffer->add_square_waveform(1.0,0.5,max,min,this->rear_right_area);
+ color.R=128;
+ color.G=0;
+ color.B=0;
+ this->frame_buffer->set_color(color,this->rear_center_area);
+ this->frame_buffer->set_color(color,this->rear_left_area);
+ }
}catch(CException &e){
+ ROS_WARN_STREAM(e.what());
this->goState(iri_base_driver::CLOSED);
}
}
@@ -305,43 +327,18 @@ void AdcCarLightsDriver::set_turn_right(void)
void AdcCarLightsDriver::set_parked(void)
{
TPixelRGB color;
- TLEDArea area;
try{
- this->frame_buffer->clear_all_patterns();
- color.R=this->config_.parked_R;
- color.G=this->config_.parked_G;
- color.B=this->config_.parked_B;
- area.min_row=this->config_.front_left_start_row;
- area.max_row=this->config_.front_left_start_row+this->config_.front_left_num_rows-1;
- area.min_col=this->config_.front_left_start_col;
- area.max_col=this->config_.front_left_start_col+this->config_.front_left_num_cols-1;
- this->frame_buffer->set_color(color,area);
- color.R=this->config_.parked_R;
- color.G=this->config_.parked_G;
- color.B=this->config_.parked_B;
- area.min_row=this->config_.front_right_start_row;
- area.max_row=this->config_.front_right_start_row+this->config_.front_right_num_rows-1;
- area.min_col=this->config_.front_right_start_col;
- area.max_col=this->config_.front_right_start_col+this->config_.front_right_num_cols-1;
- this->frame_buffer->set_color(color,area);
- color.R=this->config_.parked_R;
- color.G=this->config_.parked_G;
- color.B=this->config_.parked_B;
- area.min_row=this->config_.rear_left_start_row;
- area.max_row=this->config_.rear_left_start_row+this->config_.rear_left_num_rows-1;
- area.min_col=this->config_.rear_left_start_col;
- area.max_col=this->config_.rear_left_start_col+this->config_.rear_left_num_cols-1;
- this->frame_buffer->set_color(color,area);
- color.R=this->config_.parked_R;
- color.G=this->config_.parked_G;
- color.B=this->config_.parked_B;
- area.min_row=this->config_.rear_right_start_row;
- area.max_row=this->config_.rear_right_start_row+this->config_.rear_right_num_rows-1;
- area.min_col=this->config_.rear_right_start_col;
- area.max_col=this->config_.rear_right_start_col+this->config_.rear_right_num_cols-1;
- this->frame_buffer->set_color(color,area);
+ if(this->frame_buffer!=NULL)
+ {
+ this->frame_buffer->clear_all_patterns();
+ color.R=0;
+ color.G=0;
+ color.B=255;
+ this->frame_buffer->set_color(color,this->whole_area);
+ }
}catch(CException &e){
+ ROS_WARN_STREAM(e.what());
this->goState(iri_base_driver::CLOSED);
}
}
@@ -349,55 +346,21 @@ void AdcCarLightsDriver::set_parked(void)
void AdcCarLightsDriver::set_emergency(void)
{
TPixelRGB max,min;
- TLEDArea area;
try{
- this->frame_buffer->clear_all_patterns();
- max.R=this->config_.emergency_R;
- max.G=this->config_.emergency_G;
- max.B=this->config_.emergency_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.front_left_start_row;
- area.max_row=this->config_.front_left_start_row+this->config_.front_left_num_rows-1;
- area.min_col=this->config_.front_left_start_col;
- area.max_col=this->config_.front_left_start_col+this->config_.front_left_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.emergency_period,this->config_.emergency_period/2.0,max,min,area);
- max.R=this->config_.emergency_R;
- max.G=this->config_.emergency_G;
- max.B=this->config_.emergency_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.front_right_start_row;
- area.max_row=this->config_.front_right_start_row+this->config_.front_right_num_rows-1;
- area.min_col=this->config_.front_right_start_col;
- area.max_col=this->config_.front_right_start_col+this->config_.front_right_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.emergency_period,this->config_.emergency_period/2.0,max,min,area);
- max.R=this->config_.emergency_R;
- max.G=this->config_.emergency_G;
- max.B=this->config_.emergency_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.rear_left_start_row;
- area.max_row=this->config_.rear_left_start_row+this->config_.rear_left_num_rows-1;
- area.min_col=this->config_.rear_left_start_col;
- area.max_col=this->config_.rear_left_start_col+this->config_.rear_left_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.emergency_period,this->config_.emergency_period/2.0,max,min,area);
- max.R=this->config_.emergency_R;
- max.G=this->config_.emergency_G;
- max.B=this->config_.emergency_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.rear_right_start_row;
- area.max_row=this->config_.rear_right_start_row+this->config_.rear_right_num_rows-1;
- area.min_col=this->config_.rear_right_start_col;
- area.max_col=this->config_.rear_right_start_col+this->config_.rear_right_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.emergency_period,this->config_.emergency_period/2.0,max,min,area);
+ if(this->frame_buffer!=NULL)
+ {
+ this->frame_buffer->clear_all_patterns();
+ max.R=255;
+ max.G=64;
+ max.B=0;
+ min.R=0;
+ min.G=0;
+ min.B=0;
+ this->frame_buffer->add_square_waveform(1.0,0.5,max,min,this->whole_area);
+ }
}catch(CException &e){
+ ROS_WARN_STREAM(e.what());
this->goState(iri_base_driver::CLOSED);
}
}
@@ -405,55 +368,21 @@ void AdcCarLightsDriver::set_emergency(void)
void AdcCarLightsDriver::set_health(void)
{
TPixelRGB max,min;
- TLEDArea area;
try{
- this->frame_buffer->clear_all_patterns();
- max.R=this->config_.health_color1_R;
- max.G=this->config_.health_color1_G;
- max.B=this->config_.health_color1_B;
- min.R=this->config_.health_color2_R;
- min.G=this->config_.health_color2_G;
- min.B=this->config_.health_color2_B;
- area.min_row=this->config_.front_left_start_row;
- area.max_row=this->config_.front_left_start_row+this->config_.front_left_num_rows-1;
- area.min_col=this->config_.front_left_start_col;
- area.max_col=this->config_.front_left_start_col+this->config_.front_left_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.health_period,this->config_.health_period/2.0,max,min,area);
- max.R=this->config_.health_color1_R;
- max.G=this->config_.health_color1_G;
- max.B=this->config_.health_color1_B;
- min.R=this->config_.health_color2_R;
- min.G=this->config_.health_color2_G;
- min.B=this->config_.health_color2_B;
- area.min_row=this->config_.front_right_start_row;
- area.max_row=this->config_.front_right_start_row+this->config_.front_right_num_rows-1;
- area.min_col=this->config_.front_right_start_col;
- area.max_col=this->config_.front_right_start_col+this->config_.front_right_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.health_period,this->config_.health_period/2.0,max,min,area);
- max.R=this->config_.health_color1_R;
- max.G=this->config_.health_color1_G;
- max.B=this->config_.health_color1_B;
- min.R=this->config_.health_color2_R;
- min.G=this->config_.health_color2_G;
- min.B=this->config_.health_color2_B;
- area.min_row=this->config_.rear_left_start_row;
- area.max_row=this->config_.rear_left_start_row+this->config_.rear_left_num_rows-1;
- area.min_col=this->config_.rear_left_start_col;
- area.max_col=this->config_.rear_left_start_col+this->config_.rear_left_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.health_period,this->config_.health_period/2.0,max,min,area);
- max.R=this->config_.health_color1_R;
- max.G=this->config_.health_color1_G;
- max.B=this->config_.health_color1_B;
- min.R=this->config_.health_color2_R;
- min.G=this->config_.health_color2_G;
- min.B=this->config_.health_color2_B;
- area.min_row=this->config_.rear_right_start_row;
- area.max_row=this->config_.rear_right_start_row+this->config_.rear_right_num_rows-1;
- area.min_col=this->config_.rear_right_start_col;
- area.max_col=this->config_.rear_right_start_col+this->config_.rear_right_num_cols-1;
- this->frame_buffer->add_square_waveform(this->config_.health_period,this->config_.health_period/2.0,max,min,area);
+ if(this->frame_buffer!=NULL)
+ {
+ this->frame_buffer->clear_all_patterns();
+ max.R=255;
+ max.G=0;
+ max.B=0;
+ min.R=0;
+ min.G=0;
+ min.B=255;
+ this->frame_buffer->add_square_waveform(1.0,0.5,max,min,this->whole_area);
+ }
}catch(CException &e){
+ ROS_WARN_STREAM(e.what());
this->goState(iri_base_driver::CLOSED);
}
}
@@ -461,55 +390,21 @@ void AdcCarLightsDriver::set_health(void)
void AdcCarLightsDriver::set_charge(void)
{
TPixelRGB max,min;
- TLEDArea area;
try{
- this->frame_buffer->clear_all_patterns();
- max.R=this->config_.charge_R;
- max.G=this->config_.charge_G;
- max.B=this->config_.charge_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.front_left_start_row;
- area.max_row=this->config_.front_left_start_row+this->config_.front_left_num_rows-1;
- area.min_col=this->config_.front_left_start_col;
- area.max_col=this->config_.front_left_start_col+this->config_.front_left_num_cols-1;
- this->frame_buffer->add_sine_waveform(this->config_.charge_period,0,max,min,area);
- max.R=this->config_.charge_R;
- max.G=this->config_.charge_G;
- max.B=this->config_.charge_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.front_right_start_row;
- area.max_row=this->config_.front_right_start_row+this->config_.front_right_num_rows-1;
- area.min_col=this->config_.front_right_start_col;
- area.max_col=this->config_.front_right_start_col+this->config_.front_right_num_cols-1;
- this->frame_buffer->add_sine_waveform(this->config_.charge_period,0,max,min,area);
- max.R=this->config_.charge_R;
- max.G=this->config_.charge_G;
- max.B=this->config_.charge_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.rear_left_start_row;
- area.max_row=this->config_.rear_left_start_row+this->config_.rear_left_num_rows-1;
- area.min_col=this->config_.rear_left_start_col;
- area.max_col=this->config_.rear_left_start_col+this->config_.rear_left_num_cols-1;
- this->frame_buffer->add_sine_waveform(this->config_.charge_period,0,max,min,area);
- max.R=this->config_.charge_R;
- max.G=this->config_.charge_G;
- max.B=this->config_.charge_B;
- min.R=0;
- min.G=0;
- min.B=0;
- area.min_row=this->config_.rear_right_start_row;
- area.max_row=this->config_.rear_right_start_row+this->config_.rear_right_num_rows-1;
- area.min_col=this->config_.rear_right_start_col;
- area.max_col=this->config_.rear_right_start_col+this->config_.rear_right_num_cols-1;
- this->frame_buffer->add_sine_waveform(this->config_.charge_period,0,max,min,area);
+ if(this->frame_buffer!=NULL)
+ {
+ this->frame_buffer->clear_all_patterns();
+ max.R=0;
+ max.G=255;
+ max.B=0;
+ min.R=0;
+ min.G=0;
+ min.B=0;
+ this->frame_buffer->add_sine_waveform(2.0,0,max,min,this->whole_area);
+ }
}catch(CException &e){
+ ROS_WARN_STREAM(e.what());
this->goState(iri_base_driver::CLOSED);
}
}
diff --git a/src/adc_car_lights_driver_node.cpp b/src/adc_car_lights_driver_node.cpp
index 085e41bf1a0a83cec39cddf1c8fd6c54957b4c6e..7e3699fc316a032e33efb6eb28cdbcb393efa7e2 100644
--- a/src/adc_car_lights_driver_node.cpp
+++ b/src/adc_car_lights_driver_node.cpp
@@ -17,6 +17,9 @@ AdcCarLightsDriverNode::AdcCarLightsDriverNode(ros::NodeHandle &nh) :
// [init subscribers]
// [init services]
+ this->set_lights_server_ = this->private_node_handle_.advertiseService("set_lights", &AdcCarLightsDriverNode::set_lightsCallback, this);
+ pthread_mutex_init(&this->set_lights_mutex_,NULL);
+
// [init clients]
@@ -48,6 +51,55 @@ void AdcCarLightsDriverNode::mainNodeThread(void)
/* [subscriber callbacks] */
/* [service callbacks] */
+bool AdcCarLightsDriverNode::set_lightsCallback(iri_adc_msgs::set_car_lights::Request &req, iri_adc_msgs::set_car_lights::Response &res)
+{
+ ROS_DEBUG("AdcCarLightsDriverNode::set_lightsCallback: New Request Received!");
+
+ //use appropiate mutex to shared variables if necessary
+ this->driver_.lock();
+ //this->set_lights_mutex_enter();
+
+ if(req.lights==iri_adc_msgs::set_car_lightsRequest::CAR_OFF)
+ this->driver_.clear_all();
+ else if(req.lights==iri_adc_msgs::set_car_lightsRequest::CAR_NORMAL_DRIVE)
+ this->driver_.set_standard();
+ else if(req.lights==iri_adc_msgs::set_car_lightsRequest::CAR_BRAKE)
+ this->driver_.set_brake();
+ else if(req.lights==iri_adc_msgs::set_car_lightsRequest::CAR_TURN_LEFT)
+ this->driver_.set_turn_left();
+ else if(req.lights==iri_adc_msgs::set_car_lightsRequest::CAR_TURN_RIGHT)
+ this->driver_.set_turn_right();
+ else if(req.lights==iri_adc_msgs::set_car_lightsRequest::CAR_PARKED)
+ this->driver_.set_parked();
+ else if(req.lights==iri_adc_msgs::set_car_lightsRequest::CAR_EMERGENCY)
+ this->driver_.set_emergency();
+ else if(req.lights==iri_adc_msgs::set_car_lightsRequest::CAR_AMBULANCE)
+ this->driver_.set_health();
+ else if(req.lights==iri_adc_msgs::set_car_lightsRequest::CAR_CHARGING)
+ this->driver_.set_charge();
+ else
+ {
+ this->driver_.unlock();
+ return false;
+ }
+
+ //unlock previously blocked shared variables
+ //this->set_lights_mutex_exit();
+ this->driver_.unlock();
+
+ return true;
+}
+
+void AdcCarLightsDriverNode::set_lights_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->set_lights_mutex_);
+}
+
+void AdcCarLightsDriverNode::set_lights_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->set_lights_mutex_);
+}
+
/* [action callbacks] */
@@ -71,51 +123,6 @@ void AdcCarLightsDriverNode::node_config_update(Config& new_cfg, uint32_t level)
try{
if(new_cfg.rate!=this->getRate())
this->setRate(new_cfg.rate);
- if(new_cfg.clear_all)
- {
- this->driver_.clear_all();
- new_cfg.clear_all=false;
- }
- else if(new_cfg.standard_active)
- {
- this->driver_.set_standard();
- new_cfg.standard_active=false;
- }
- else if(new_cfg.brake_active)
- {
- this->driver_.set_brake();
- new_cfg.brake_active=false;
- }
- else if(new_cfg.turn_left_active)
- {
- this->driver_.set_turn_left();
- new_cfg.turn_left_active=false;
- }
- else if(new_cfg.turn_right_active)
- {
- this->driver_.set_turn_right();
- new_cfg.turn_right_active=false;
- }
- else if(new_cfg.parked_active)
- {
- this->driver_.set_parked();
- new_cfg.parked_active=false;
- }
- else if(new_cfg.emergency_active)
- {
- this->driver_.set_emergency();
- new_cfg.emergency_active=false;
- }
- else if(new_cfg.health_active)
- {
- this->driver_.set_health();
- new_cfg.health_active=false;
- }
- else if(new_cfg.charge_active)
- {
- this->driver_.set_charge();
- new_cfg.charge_active=false;
- }
this->config_=new_cfg;
}catch(CException &e){
ROS_WARN_STREAM(e.what());
@@ -126,6 +133,7 @@ void AdcCarLightsDriverNode::node_config_update(Config& new_cfg, uint32_t level)
AdcCarLightsDriverNode::~AdcCarLightsDriverNode(void)
{
// [free dynamic memory]
+ pthread_mutex_destroy(&this->set_lights_mutex_);
}
/* main function */