proximity_sensor_driver.cpp

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Robert Bosch LLC.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Robert Bosch nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *********************************************************************/

//\Author Philip Roan and Joerg Wagner, Bosch LLC

//Common Headers
#include <iostream>
#include <sstream>
#include <string.h>
#include <cmath>

//ROS Headers
#include <ros/ros.h>
#include <ros/time.h>

#include "proximity_sensor_driver.h"

//TODO:
//  - implement a useful application with dynamic reconfigure or delete the code belonging to dynamic reconfigure
//      - change the software to handle 192 instead of 191 sensors
//      - bug-fix: After the execution of command "Ram to Flash" the dynamic threshold is changed to zero.
//        In the most cases the dynamic threshold could be reset to the old value by restarting the ECU (or several restarts).
//      - Increase the I2C-BUS speed of the ECU (Must be changed in the assembler code of the ECU)
//                      *An increased I2C-BUS speed could slow down the evaluation software of the sensor skin
//      - Implementation of error handling routines

ProxSensor::ProxSensor( const ros::NodeHandle& nh ):  // Initialization List Follows
  skin_connected_( false ),
  serialNumber_( "         " ),
  i2c_frequency_( 100000 ),
  cell_count_( 0 )
{
  int noise = -1;
  int threshold = -1;
  int dynamic_threshold = -1;

  // Getting parameters from .launch file
  nh.getParam( "/prox_sensor_driver/sensor_count", cell_count_ );
  nh.getParam( "/prox_sensor_driver/noise_threshold", noise );
  nh.getParam( "/prox_sensor_driver/static_threshold", threshold );
  nh.getParam( "/prox_sensor_driver/dynamic_threshold", dynamic_threshold );

  if( noise < 0 || noise > 255 )
  {
    noise_ = 0;
  }
  else
  {
    noise_ = (uint8_t)noise;
  }
  if( threshold < 0 || threshold > 255 )
  {
    threshold_ = THRESHOLD_DEFAULT;
  }
  else
  {
    threshold_ = (uint8_t)threshold;
  }
  if( dynamic_threshold < 0 || dynamic_threshold > 255 )
  {
    dynamic_threshold_ = THRESHOLD_DEFAULT;
  }
  else
  {
    dynamic_threshold_ = (uint8_t)dynamic_threshold;
  }

  //ROS_INFO("Retrieved parameters: %d %d %d", sensor_count_, noise_, threshold_);
}

ProxSensor::~ProxSensor()
{
  if( skin_connected_ == true )
  {
    // Close SUB20-Device device
    sub_close( handle_ );
    //Reset status
    skin_connected_ = false;
  }
  ROS_DEBUG( "Sub20 closed." );
}

bool ProxSensor::init( void )
{

  int error_code = 0; //Error code for Sub20 API
  bool device_found = false; 

  subdev_ = sub_find_devices( NULL ); // start a new search for Sub20 devices
  while( subdev_ != NULL )
  {
    handle_ = sub_open( subdev_ ); // open found subdevice
    
    if( handle_ == 0 ) // on success, sub_open returns non-zero handle
    {
      //sub_open was not successful
      ROS_ERROR( "sub_open: %s", sub_strerror(sub_errno) );
      return false;
    }
  
    // Subdevice successfully opened
    ROS_DEBUG( "Device Handle: %ld", (long)handle_ );
    // Read Serial number of subdevice
    if( sub_get_serial_number( handle_, const_cast<char*>(serialNumber_.c_str() ), serialNumber_.size() ) >= 0 )
    {
      ROS_DEBUG( "Serial Number: %s", serialNumber_.c_str() );
    }

    ROS_DEBUG( "Initializing I2C interface to %d Hz.", i2c_frequency_ );
    error_code = sub_i2c_freq( handle_, &i2c_frequency_ ); // Set I2C frequency
    if( error_code != 0 )
    {
      ROS_ERROR( "Sub20 I2C configuration failed. Status: 0x%02x.", sub_i2c_status );
      sub_close( handle_ );
      return false;
    }
    else
    {
      ROS_DEBUG( "I2C initialized. Communicating at %d Hz.", i2c_frequency_ );
    }

    // Scan for I2C slave devices
    ROS_DEBUG( "Scanning for I2C devices." );
    int num_i2c_devices;        // number of i2c slave devices attached to this Sub20
    char device_addresses[112]; // i2c has a max of 112 devices per bus
    
    error_code = sub_i2c_scan( handle_, &num_i2c_devices, device_addresses ); //scan for devices
    if( error_code != 0 )
    {
      ROS_ERROR( "No I2C devices connected to Sub20." );
      sub_close( handle_ );
      return false;
    }
    ROS_DEBUG( "Number of I2C slaves detected: %d", num_i2c_devices );
    for( int i = 0; i < num_i2c_devices; i++ )
    {
      ROS_DEBUG( "%d. Slave address: 0x%02x", i+1, int(device_addresses[i]) ); // display found devices
      
      //verify that an I2C device exists at the skin ECU address
      if( PROX_SENSOR_I2C_ADR == (int)device_addresses[i] )
      {
        device_found = true;
        ROS_DEBUG( "I2C device at skin ECU address (0x%02x) exists.", PROX_SENSOR_I2C_ADR );
      }
    }
    if( device_found == false )
    {
      ROS_ERROR( "No I2C device at address 0x%02x.", PROX_SENSOR_I2C_ADR );
      sub_close( handle_ );
      return false;
    }

          
    char buffer[8] = {0};
    char adr[2];
    uint16_t next_address = 0;
    int adr_length = 0;

    adr[0] = DATA_ADR_HI;
    adr[1] = DATA_ADR_LO;
    adr_length = 2;
    error_code = sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, adr, adr_length ); //Memory Address Write
    ros::Duration(0.05).sleep(); //sleep for 0.05 seconds
          
    error_code += sub_i2c_read( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, buffer, 8 );//read data (8 bytes)
    ROS_DEBUG( "Status read from I2C device: %d", sub_i2c_status );
    
    if( error_code == 0 && ( ((int)buffer[0] & 0xFF) < 64 ) ) //condition for sensor skin: value stat < 64
    {
      ROS_DEBUG( "Connected device is a sensor skin." );
      skin_connected_ = true;
    }
    else
    {
      ROS_ERROR( "Found sensor is not a sensor skin." );
      sub_close( handle_ );
      return false;
    }

    // determine number of sensor elements
    // test each sensor until END_OF_CHAIN_MARKER is found 
    ROS_INFO("Determining the number of skin cells. We need at most %d I2C transmissions, with %d sensors per I2C transmission.", MAX_SENSORS / SENSORS_PER_I2C, SENSORS_PER_I2C );
    for( int i = 0; i < MAX_SENSORS / SENSORS_PER_I2C; i++ ) //read sensor values in blocks
    {
      next_address = ( i * SENSORS_PER_I2C * sizeof(struct skin_cell) ) + ( (uint16_t)DATA_ADR_HI << 8 | (uint16_t)DATA_ADR_LO );
      adr[1] = (char)next_address;
      adr[0] = (char)(next_address >> 8);
      adr_length = 2;
      error_code = sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, adr, adr_length ); //Memory Address Write
      ros::Duration(0.05).sleep(); //sleep for 0.05 seconds
      error_code += sub_i2c_read( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, (char*)&cells_[i*SENSORS_PER_I2C], MAX_BYTES_PER_I2C );//read data
      if( error_code != 0 )
      {
        ROS_ERROR("Cannot read data from ECU to determine number of cells.");
        sub_close( handle_ );
        return false;
      }
      for( int  k = i*SENSORS_PER_I2C; k < (i+1)*SENSORS_PER_I2C; k++ )
      {
        //ROS_DEBUG( "cell:%d\tstatus:%02x value:%03d offset:%03d limit:%03d V1:%03d V2:%03d V3:%03d dynamic:%d", k, cells_[k].status, cells_[k].value, cells_[k].offset, cells_[k].limit, cells_[k].U1, cells_[k].U2, cells_[k].U3, cells_[k].dynamic );
        if( cells_[k].limit == END_OF_CHAIN_MARKER || k >= MAX_SENSORS ) //|| (cells_[k].U1 == 0 && cells_[k].U2 == 0 && cells_[k].U3 == 0) ) // true for last skin cell
        {
          if( k != cell_count_ )
          {
            ROS_WARN( "Number of proximity sensors detected(%d) does not match the number specified(%d).", k, cell_count_ );
          }
          else
          {
            ROS_DEBUG( "Found %d attached proximity sensors.", k );

            // write the number of sensors to RAM of device ?
            // reinit ?
          }
          
          // initialize proximity sensor
          //sendToECU( INITIALIZE, 0, 0 );
          return true;
        }
      }
    }

    // search for the next Sub20 device and loop
    subdev_ = sub_find_devices( subdev_ );
  }


  ROS_ERROR("No Sub20 device found.");
  return false;
}



bool ProxSensor::getMeasurement( void )
{
  int  error_code;
  char skin_status[2] = {0};
  char adr[2] = {0};
  int adr_length = 0;

  //only execute if SubDevice is configured and I2C device found
  if( skin_connected_ )
  {
    error_code = 0;
   
    // Read 2-byte status
    // send: Start|Slave Adr.|write|Mem. Adr. MSB/Hi|Mem. Adr. LSB/Lo|Stop
    adr[0] = STATUS_ADDRESS_HIGH;
    adr[1] = STATUS_ADDRESS_LOW;
    adr_length = 2;
    error_code += sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, adr, adr_length );
    ros::Duration(0.05).sleep();//sleeps for 0.05 seconds (50msec sleep time required for ECU|uC of ECU is to slow to handle a I2C communication without sleep time)

    // Receive data from ECU (Start|Slave Adr.|Data[0]|...|Stop)
    error_code += sub_i2c_read( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, skin_status, STATUS_LENGTH );//read data (2 bytes)

    // Extract data
    dynamic_threshold_ = (uint8_t)( (skin_status[1]) & 0xFF ); //Dynamic threshold initialized
    //Status byte contains Go/NoGo (Bit0) and Err/NoErr (Bit1)
    status_go_  = ( (skin_status[0] & 0x01) == 0 ) ? true : false;
    status_err_ = ( (skin_status[0] & 0x02) == 0 ) ? true : false;
    //ROS_DEBUG( "Go/NoGo: %d, Err: %d", status_go_, status_err_ );
    
    // Read sensor data
    //int old_read_loops = (int)ceil( sizeof(struct skin_cell) * cell_count_ / (double)MAX_BYTES_PER_I2C );
    int read_loops = cell_count_ / SENSORS_PER_I2C;
    uint16_t next_address = 0;
    //ROS_INFO("old: %d, new: %d", old_read_loops, read_loops);
    for( int k = 0; k < read_loops; k++ )
    {
      //send: Start|Slave Adr.|write|Mem. Adr. MSB/Hi|Mem. Adr. LSB/Lo|Stop
      next_address = ( k * SENSORS_PER_I2C * sizeof(struct skin_cell) ) + ( (uint16_t)DATA_ADR_HI << 8 | (uint16_t)DATA_ADR_LO );
      adr[1] = (uint8_t)next_address;
      adr[0] = (uint8_t)(next_address >> 8);
      adr_length = 2;
      ROS_DEBUG("Reading %d sensors (%d bytes). ECU start address: 0x%02x%02x to memory address start 0x%x", SENSORS_PER_I2C, SENSORS_PER_I2C * sizeof(struct skin_cell), adr[0], (uint8_t)adr[1], &cells_[k*SENSORS_PER_I2C] );
      adr_length = 2;
      error_code += sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, adr, adr_length ); //Memory Address Write
      //ROS_DEBUG( "sub_i2c_write status: %d", sub_i2c_status );
      ros::Duration(0.05).sleep();//sleeps for 0.05 seconds (50msec sleep time required for ECU|uC of ECU is to slow to handle a I2C communication without sleep time)

      //receive data from ECU (Start|Slave Adr.|Data[0]|...|Stop)
      error_code += sub_i2c_read( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, (char*)&cells_[k*SENSORS_PER_I2C], MAX_BYTES_PER_I2C );//read data
      //ROS_DEBUG( "sub_i2c_read status: %d", sub_i2c_status );
      //ros::Duration(0.05).sleep();//sleeps for 0.05 seconds (50msec sleep time required for ECU|uC of ECU is to slow to handle a I2C communication without sleep time)
    }
    if( cell_count_ % SENSORS_PER_I2C != 0 ) //read data of remaining sensors
    {
      //send: Start|Slave Adr.|write|Mem. Adr. MSB/Hi|Mem. Adr. LSB/Lo|Stop
      next_address = ( read_loops * SENSORS_PER_I2C * sizeof(struct skin_cell) ) + ( (uint16_t)DATA_ADR_HI << 8 | (uint16_t)DATA_ADR_LO );
      adr[1] = (char)next_address;
      adr[0] = (char)(next_address >> 8);
      adr_length = 2;
      ROS_DEBUG("Reading %d sensors (%d bytes). ECU start address: 0x%02x%02x to memory address start 0x%x", cell_count_ % SENSORS_PER_I2C, cell_count_ % SENSORS_PER_I2C * sizeof(struct skin_cell), adr[0], (uint8_t)adr[1], &cells_[read_loops*SENSORS_PER_I2C] );
      adr_length = 2;
      error_code += sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, adr, adr_length ); //Memory Address Write
      //ROS_DEBUG( "sub_i2c_write status: %d", sub_i2c_status );
      ros::Duration(0.05).sleep();//sleeps for 0.05 seconds (50msec sleep time required for ECU|uC of ECU is to slow to handle a I2C communication without sleep time)
      
      //receive data from ECU (Start|Slave Adr.|Data[0]|...|Stop)
      error_code += sub_i2c_read( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, (char*)&cells_[read_loops*SENSORS_PER_I2C], cell_count_ % SENSORS_PER_I2C * sizeof(struct skin_cell) );
      //ROS_DEBUG( "sub_i2c_read status: %d", sub_i2c_status );
      //ros::Duration(0.05).sleep();//sleeps for 0.05 seconds (50msec sleep time required for ECU|uC of ECU is to slow to handle a I2C communication without sleep time)
    }
   
    
    if( error_code != 0 )
    {
      ROS_WARN("getMeasurement(): master read transaction failed. Error code %d. Status %d", error_code, sub_i2c_status);
      return false;
    }
    else
    {
      ROS_DEBUG("Measurement available: %d\tStatus %02x\tOffset %d\tLimit %d", cells_[0].value, cells_[0].status, cells_[0].offset, cells_[0].limit);
    }
  }
  else
  {
    ROS_WARN("Measurement was not retrieved.");
    return false;
  }

  return true;
}


void ProxSensor::checkConnection( void )
{
  skin_connected_ = false;
  sub_close( handle_ );
}




// Document me!
bool ProxSensor::sendToECU( ECU_command command, uint8_t sensor_number, uint8_t value )
{
  int error_code = 0;
  int sensor_address = 0;
  char data = 0;
  char adr[2] = {0};
  int adr_length = 0;

  if( skin_connected_ )
  {
    switch( command )
    {
    case INITIALIZE:
      data = 1;
      error_code = sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, (int)COMMAND_ADR, RAM_ADR_LENGTH, &data, RAM_WRITE_LENGTH );
      break;
    case WRITE_TO_FLASH:
      data = 4;
      
      adr[1] = (uint8_t)COMMAND_ADR;
      adr[0] = (uint8_t)(COMMAND_ADR >> 8);
      adr_length = 2;
      //error_code = sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, 
      error_code = sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, adr, adr_length ); //Memory Address Write
      ros::Duration(0.05).sleep();//sleeps for 0.05 seconds (50msec sleep time required for ECU|uC of ECU is to slow to handle a I2C communication without sleep time)
      error_code += sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, 0x00, SKIP_MEM_ADR_WRITE, &data, RAM_WRITE_LENGTH );
      //error_code = sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, (int)COMMAND_ADR, RAM_ADR_LENGTH, &data, RAM_WRITE_LENGTH );
      break;
    case SET_DYNAMIC_THRESHOLD:
      data = value;
      error_code = sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, (int)DYNAMIC_THRESHOLD_ADR, RAM_ADR_LENGTH, &data, RAM_WRITE_LENGTH );
      break;
    case SET_STATIC_THRESHOLD:
      data = value;
      sensor_address = (int)( STATIC_THRESHOLD_ADR + (sensor_number)*sizeof(struct skin_cell) );
      error_code = sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, sensor_address, RAM_ADR_LENGTH, &data, RAM_WRITE_LENGTH );
      break;
    case SET_END_OF_CHAIN:
      // clear old END_OF_CHAIN_MARKER, where 'value' contains the sensor number of the old end of chain
      data = (char)threshold_;
      sensor_address = (int)( STATIC_THRESHOLD_ADR + value*sizeof(struct skin_cell) );
      error_code = sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, sensor_address, RAM_ADR_LENGTH, &data, RAM_WRITE_LENGTH );
      ros::Duration(0.05).sleep();
      
      data = END_OF_CHAIN_MARKER;
      sensor_address = (int)( STATIC_THRESHOLD_ADR + sensor_number*sizeof(struct skin_cell) );
      error_code += sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, sensor_address, RAM_ADR_LENGTH, &data, RAM_WRITE_LENGTH );
      ros::Duration(0.05).sleep();
      
      sendToECU(ACTIVATE_SENSOR, value, 0);
      ros::Duration(0.05).sleep();
      sendToECU(DEACTIVATE_SENSOR, sensor_number, 0);
      break;
    case ACTIVATE_SENSOR:
      data = ACTIVE;
      sensor_address = (int)( ACTIVATE_ADR + (sensor_number)*sizeof(struct skin_cell) );
      error_code += sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, sensor_address, RAM_ADR_LENGTH, &data, RAM_WRITE_LENGTH);
      break;
    case DEACTIVATE_SENSOR:
      data = INACTIVE;
      sensor_address = (int)( ACTIVATE_ADR + (sensor_number)*sizeof(struct skin_cell) );
      error_code += sub_i2c_write( handle_, PROX_SENSOR_I2C_ADR, sensor_address, RAM_ADR_LENGTH, &data, RAM_WRITE_LENGTH);
      break;
    default:
      ROS_WARN("Received unknown command for proximity sensor ECU: %d", command);
      return false;
    }

    if( error_code != 0 )
    {
      ROS_ERROR("Command not sent to ECU. Command: %d. Error code: %d. I2C Status: %d.", command, error_code, sub_i2c_status);
      return false;
    }
    else
    {
      ROS_DEBUG("Command (%d) sent to ECU.", command);
      return true;
    }
  }

  ROS_ERROR("Command was not sent because proximity sensor was not connected."); 
  return false;
}



//dynamic reconfigure routine
void ProxSensor::reconfigure_callback(proximity_sensor_driver::ProximitySensorConfig &config, uint32_t level)
{

  ROS_DEBUG("Reconfigure request, update?: %s", config.apply_changes?"yes":"no");
  

  if( config.apply_changes == true )
  {
    // update number of sensors
    if( config.sensor_count != cell_count_ )
    {
      if( sendToECU( SET_END_OF_CHAIN, (uint8_t)config.sensor_count, cell_count_ ) != true )
        ROS_ERROR("Attemped to change the number of sensors, but failure in sendToECU");

      cell_count_ = (uint8_t)config.sensor_count;
    }

    // update noise threshold
    if( config.noise_threshold != noise_ )
    {
      noise_ = (uint8_t)config.noise_threshold;
    }
    
    // update static thresholds
    if( config.change_static_threshold == true )
    {
      if( sendToECU( SET_STATIC_THRESHOLD, (uint8_t)config.static_threshold_sensor-1, (uint8_t)config.static_threshold ) != true )
        ROS_ERROR("Attemped to set the static threshold of sensor %d, but failure in sendToECU", config.static_threshold_sensor);
    }
    
    // update dynamic threshold
    if( config.dynamic_threshold != dynamic_threshold_ )
    {
      dynamic_threshold_ = (uint8_t)config.dynamic_threshold;

      if( sendToECU( SET_DYNAMIC_THRESHOLD, 0, dynamic_threshold_ ) != true )
        ROS_ERROR("Attemped to set the dynamic threshold, but failure in sendToECU");
    }
    
    // activate sensor
    if( config.activate_sensor == true )
    {
      if( sendToECU( ACTIVATE_SENSOR, (uint8_t)config.activate_sensor_number-1, 0 ) != true )
        ROS_ERROR("Attemped to activate sensor %d, but failure in sendToECU", config.activate_sensor_number);
    }

    // deactivate sensor
    if( config.deactivate_sensor == true )
    {
      if( sendToECU( DEACTIVATE_SENSOR, (uint8_t)config.deactivate_sensor_number-1, 0 ) != true )
        ROS_ERROR("Attemped to deactivate sensor %d, but failure in sendToECU", config.deactivate_sensor_number);
    }

    // write changes to flash memory
    if( config.write_to_flash == true )
    {
      ros::Duration(0.05).sleep();
      if( sendToECU( WRITE_TO_FLASH, 0, 0 ) != true )
        ROS_ERROR("Attemped to write to flash, but failure in sendToECU");
    }

    // reinitialize ECU
    if( config.initialize == true )
    {
      ros::Duration(0.05).sleep();
      if( sendToECU( INITIALIZE, 0, 0 ) != true )
        ROS_ERROR("Attemped to reinitialize, but failure in sendToECU");
    }
  }
}

//reinitialize service callback
bool ProxSensor::reinitialize_callback(proximity_sensor_driver::reinitialize_proximity_sensor::Request &req,
                                 proximity_sensor_driver::reinitialize_proximity_sensor::Response &res)
{
  if( sendToECU( INITIALIZE, 0, 0 ) != true )
  {
    ROS_ERROR("Attemped to reinitialize, but failure in sendToECU");
    return false;
  }
  return true;
}


// ----------
// -- MAIN --
// ----------
int main(int argc, char **argv)
{
  // ros init
  ros::init(argc, argv, "proximity_data_publisher");
  ros::NodeHandle nh;
  
  // create a skin class
  ProxSensor pr2_prox_sensor_arm(nh);

  proximity_sensor_driver::prox_sensor_measurement prox_msg;//ROS message
  proximity_sensor_driver::proximity_sensor_measurement proximity_msg; //ROS message
  
  ros::Publisher prox_pub = nh.advertise<proximity_sensor_driver::prox_sensor_measurement> ("prox_sensor_data", 20);
  ros::Publisher proximity_pub = nh.advertise<proximity_sensor_driver::proximity_sensor_measurement> ("proximity_sensor_data", 20);
  
  //dynamic reconfigure
  dynamic_reconfigure::Server<proximity_sensor_driver::ProximitySensorConfig> reconfigure_serv;
  dynamic_reconfigure::Server<proximity_sensor_driver::ProximitySensorConfig>::CallbackType dr = boost::bind( &ProxSensor::reconfigure_callback, &pr2_prox_sensor_arm, _1, _2 );
  reconfigure_serv.setCallback(dr);

  ros::ServiceServer prox_serv = nh.advertiseService( "reinitialize_proximity_sensor", &ProxSensor::reinitialize_callback, &pr2_prox_sensor_arm );
  

  int temporary_counter = 0;

  while( nh.ok() )
  {
    // Wait for sensor skin to be attached and configured (1 Hz loop rate)
    {
      ros::Rate loop_rate(1);
      while( nh.ok() && (pr2_prox_sensor_arm.init() == false) )
      {
        prox_msg.is_connected = false;
        prox_msg.cell_count = 0;
        prox_msg.header.stamp = ros::Time::now();
        prox_pub.publish( prox_msg );

        proximity_msg.is_connected = false;
        proximity_msg.cell_count = 0;
        proximity_msg.value.clear();
        proximity_msg.offset.clear();
        proximity_msg.limit.clear();
        proximity_msg.header.stamp = ros::Time::now();
        proximity_pub.publish( proximity_msg );
        
        ros::spinOnce();
        loop_rate.sleep();
      }
    }

    // If sensor skin is configured, publish readings (5 Hz loop rate);
    {
      ros::Rate loop_rate(5);
      while( nh.ok() && pr2_prox_sensor_arm.skin_connected_ )
      {
        //Publish if measurement was successful
        if( pr2_prox_sensor_arm.getMeasurement() == true )
        {
          prox_msg.is_connected = true;
          prox_msg.cell_count = pr2_prox_sensor_arm.cell_count_;

          proximity_msg.is_connected = true;
          proximity_msg.cell_count = pr2_prox_sensor_arm.cell_count_;
          proximity_msg.value.clear();
          proximity_msg.offset.clear();
          proximity_msg.limit.clear();



          // !!!!! NEXT 2 LINES CORRESPOND TO THE DIRTY HACK !!!!!
          bool bool_publish = true;
          int int_strange_readings = 0;

          for( int i = 0; i < pr2_prox_sensor_arm.cell_count_; i++ )
            //for( int i = 0; i < 192; i++ )
          {
            prox_msg.value[i] = pr2_prox_sensor_arm.cells_[i].value;
            prox_msg.offset[i] = pr2_prox_sensor_arm.cells_[i].offset;
            prox_msg.limit[i] = pr2_prox_sensor_arm.cells_[i].limit;

            
            proximity_msg.value.push_back( pr2_prox_sensor_arm.cells_[i].value );
            proximity_msg.offset.push_back( pr2_prox_sensor_arm.cells_[i].offset );
            proximity_msg.limit.push_back( pr2_prox_sensor_arm.cells_[i].limit );

            
            prox_msg.debug_voltage1[i] = pr2_prox_sensor_arm.cells_[i].U1;
            prox_msg.debug_voltage2[i] = pr2_prox_sensor_arm.cells_[i].U2;
            prox_msg.debug_voltage3[i] = pr2_prox_sensor_arm.cells_[i].U3;
            //prox_msg.status[i] = pr2_prox_sensor_arm.cells_[i].status;

            // !!!!! THIS IS A DIRTY HACK, BECAUSE THE PROX_SENSOR SOMETIMES GIVES WRONG READINGS !!!!!
            //if( abs(prox_msg.value[i] - prox_msg.offset[i]) > 20 || prox_msg.value[i] < 15)
            //{
            //  int_strange_readings++;
            //  if(int_strange_readings > 10)
            //  {
            //    bool_publish = false;
            //  }
            //}
            //if(prox_msg.value[i] == 0)
            //{
            //  // !!!!! SETS PATCHES THAT GIVE ZERO READINGS TO THEIR INIT !!!!!
            //  prox_msg.value[i] = prox_msg.offset[i];
            //}




            //if( temporary_counter % 5 == 0 )
            //  ROS_DEBUG("cell:%d\tstatus:%02x value:%03d offset:%03d limit:%03d\tV1:%03d V2:%03d V3:%03d dynamic:%d", i, pr2_prox_sensor_arm.cells_[i].status, pr2_prox_sensor_arm.cells_[i].value, pr2_prox_sensor_arm.cells_[i].offset, pr2_prox_sensor_arm.cells_[i].limit, pr2_prox_sensor_arm.cells_[i].U1, pr2_prox_sensor_arm.cells_[i].U2, pr2_prox_sensor_arm.cells_[i].U3, pr2_prox_sensor_arm.cells_[i].dynamic);

          }
          prox_msg.header.stamp = ros::Time::now();
          prox_msg.status_go = pr2_prox_sensor_arm.status_go_;
          prox_msg.status_err = pr2_prox_sensor_arm.status_err_;
          
          proximity_msg.header.stamp = ros::Time::now();
          proximity_msg.status_go = pr2_prox_sensor_arm.status_go_;
          proximity_msg.status_err = pr2_prox_sensor_arm.status_err_;

          if( bool_publish )
          {
            prox_pub.publish( prox_msg );
            proximity_pub.publish( proximity_msg );
          }
        }
        else // check if skin is still connected
        {
          ROS_WARN("Proximity sensors have been disconnected.");
          pr2_prox_sensor_arm.checkConnection();

          prox_msg.is_connected = false;
          prox_msg.cell_count = 0;
          prox_msg.header.stamp = ros::Time::now();
          prox_pub.publish( prox_msg );

          proximity_msg.is_connected = false;
          proximity_msg.cell_count = 0;
          proximity_msg.header.stamp = ros::Time::now();
          proximity_pub.publish( proximity_msg );
        }


        ++temporary_counter;

        ros::spinOnce();
        loop_rate.sleep();
      }
    }
  }
  return 0;
}