Added a service to set the desired car lights.

The intermitent lights have to be improved.

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)
 # ******************************************************************** 

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. 

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"))

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
 

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

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]
 

launch/node.launch

 <?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>

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"

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 -->

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 */