Commit e3406fd9 authored by Christopher Lang's avatar Christopher Lang
Browse files

Merge branch 'sync_paramter_states' into 'master'

Sync paramter states

See merge request toposens/public/ros-packages!78
parents f4598ae4 69c8d163
......@@ -30,6 +30,12 @@ enum TsParam
ScanMode = 0b01000000, /**< Scan Mode of Sensor: 0 for scanning continuously; 1 for scanning once; 2 for listening only (once) */
};
enum TsService
{
FirmwareConfiguration,
FirmwareVersion
};
/** @brief Generates firmware-compatible commands for tuning
* performance parameters.
*
......@@ -48,17 +54,28 @@ class Command
{
public:
/** Builds a command message accepted by the TS firmware.
* @param param Setting name from the enumerated command list.
* @param param Setting name from the enumerated TsParam list.
* @param value Desired integer value for sensor parameter.
*/
Command(TsParam param, float value);
/** Builds a command message accepted by the TS firmware to invoke a service.
* @param param Service name from the enumerated TsService list.
*/
Command(TsService service);
/**
* Returns this object's parameter enumeration value denoting the firmware parameter to be updated.
* @return parameter
*/
TsParam getParam(){ return _param; }
/**
* Returns this object's parameter value denoting the parameter value to be set.
* @return value
*/
float getValue(){ return _value; }
/** Returns the latest command message produced by generate().
* @returns Pointer to a char array containing command.
*/
......@@ -78,6 +95,7 @@ class Command
char _bytes[50]; /**< Large enough buffer to hold a well-formed command.*/
static const char kPrefix = 'C'; /**< Designates a string as a firmware command.*/
TsParam _param; /**< Parameter to be encoded in command bytes.*/
float _value; /**< Parameeter value to be encoded in command bytes. */
/** Looks up command keys defined by the TS firmware corresponding to
* given setting parameters.
......@@ -86,6 +104,12 @@ class Command
*/
std::string _getKey(TsParam param);
/** Looks up service keys defined by the TS firmware.
* @param param service name from the enumerated command list.
* @returns Corresponding firmware service key.
*/
std::string _getKey(TsService service);
};
} // namespace toposens_driver
......
......@@ -7,6 +7,7 @@
#define SENSOR_H
#include <ros/ros.h>
#include <boost/thread/recursive_mutex.hpp>
#include <dynamic_reconfigure/server.h>
#include <toposens_driver/serial.h>
......@@ -99,6 +100,48 @@ class Sensor
*/
void _parse(const std::string &frame);
/**
* Examine acknowledgement message and manages result of configurations update.
* A valid acknowledgement will confirm the send parameter settings command, e.g. the command "CsPuls00003"
* should result in following valid acknowledgement "S000004C00003E".
*
*
* @param cmd instance of parameter update command
* @param acknowledgement message string
* @return true if acknowledgement verifies parameter update, false otherwise
*/
bool _evaluateAck(Command &cmd, const std::string &frame);
/**
* Parses acknowledgement message into a Command object.
*
* An acknowledgement frame corresponds to a single parameter update and has the following format:
* @n - Starts with char 'S'
* @n - 6 bytes of bit shifts to decode parameter level as defined in Command::Parameters,
* '-1' denotes unknown parameter
* @n - Char 'C', indicates a command acknowledgement frame
* @n - 5 bytes of firmware parameter value
* @n - Ends with char 'E'
*
* @param acknowledgement message string
* @return Command representing the values in the acknowledgement message
*/
Command* _parseAck(const std::string &data);
/** Updates parameter value on the config server.
* @param parameter to be updated
* @param value to update parameter within
*/
void _updateConfig(TsParam param, int value);
/** Synchronizes parameter values on the config server with values used by the firmware.
*/
void _synchronizeParamterValues();
/** Displays current firmware version of the sensor. */
void _displayFirmwareVersion();
/** Efficiently converts a char array representing a signed integer to
* its numerical value.
* @param i String iterator representing an integer value.
......@@ -110,6 +153,7 @@ class Sensor
std::string _frame_id; /**< Frame ID assigned to TsScan messages.*/
TsDriverConfig _cfg; /**< Maintains current values of all config params.*/
std::unique_ptr<Cfg> _srv; /**< Pointer to config server*/
boost::recursive_mutex mutex; /**< Mutex to control access to config server */
ros::Publisher _pub; /**< Handler for publishing TsScans.*/
std::unique_ptr<Serial> _serial; /**< Pointer for accessing serial functions.*/
......
......@@ -38,17 +38,19 @@ class Serial
* @param cmd Instance of command class specifying a parameter to be updated and the desired value.
* @returns True if sensor handshake is received to confirm settings update, false otherwise.
*/
bool sendCmd(Command cmd);
private:
int _fd; /**< Linux file descriptor pointing to TS device port.*/
std::string _port; /**< Stored device port for future access.*/
const unsigned int kBaud = B576000; /**< Baud rate needed for TS device comms.*/
void sendCmd(Command cmd, std::stringstream &data);
/**
* Blocks execution of driver until an acknowledgement message is received or watchdog timer expires.
* A valid acknowledgement will confirm the send parameter settings command, e.g. the command "CsPuls00003"
* should result in following valid acknowledgement "S000004C00003E".
*
*
* @param buffer stringstream to be used for messaging
* @return true if acknowledgement is received, false if watchdog terminated blocking mode
*/
bool waitForAcknowledgement(std::stringstream &buffer);
/**
* Checks whether frame string is an acknowledgement message.
*
* An acknowledgement frame corresponds to a single parameter update and has the following format:
* @n - Starts with char 'S'
......@@ -58,10 +60,15 @@ class Serial
* @n - 5 bytes of firmware parameter value
* @n - Ends with char 'E'
*
* @param cmd instance of parameter update command
* @return true if acknowledgement verifies parameter update, false otherwise
* @param frame string received from sensor
* @return true if acknowledgement is received, false if watchdog terminated blocking mode
*/
bool _waitForAcknowledgement(Command cmd);
bool isAcknowledgementFrame(std::string frame);
private:
int _fd; /**< Linux file descriptor pointing to TS device port.*/
std::string _port; /**< Stored device port for future access.*/
const unsigned int kBaud = B576000; /**< Baud rate needed for TS device comms.*/
};
......
......@@ -36,12 +36,28 @@ Command::Command(TsParam param, float value)
ROS_WARN_STREAM("Out of range value " << (param==TsParam::ExternalTemperature ? value/10 : value) << " clipped to closest limit");
value = (value < MIN_VALUE) ? MIN_VALUE : MAX_VALUE;
}
_value = value;
std::sprintf( _bytes, format.c_str(), kPrefix, _getKey(param).c_str(), ((int)_value));
}
/** Compiles command string for service requests.
*
* Command format:
* @n - Starts with #kPrefix
* @n - 5 bytes defining firmware service
* @n - Terminating carriage return
*/
Command::Command(TsService service)
{
memset(&_bytes, '\0', sizeof(_bytes));
std::string format = "%c%s\r";
std::sprintf( _bytes, format.c_str(), kPrefix, _getKey(param).c_str(), ((int)value));
_value = 0;
std::sprintf( _bytes, format.c_str(), kPrefix, _getKey(service).c_str());
}
/** Command keys are 5-byte strings hard-coded in the TS device firmware. */
/** Paramter keys are 5-byte strings hard-coded in the TS device firmware. */
std::string Command::_getKey(TsParam param)
{
if (param == TsParam::NumberOfPulses) return "sPuls";
......@@ -50,6 +66,15 @@ std::string Command::_getKey(TsParam param)
else if (param == TsParam::ExternalTemperature) return "sTemp";
else if (param == TsParam::NoiseIndicatorThreshold) return "sNois";
else if (param == TsParam::ScanMode) return "sMode";
return "";
}
/** Command keys are 5-byte strings hard-coded in the TS device firmware. */
std::string Command::_getKey(TsService service)
{
if (service == TsService::FirmwareConfiguration) return "gConf";
else if (service == TsService::FirmwareVersion) return "gVers";
return "";
}
/** Command keys are 5-byte strings hard-coded in the TS device firmware. */
......@@ -61,6 +86,7 @@ std::string Command::getParamName()
else if (this->_param == TsParam::ExternalTemperature) return "Calibration temperature";
else if (this->_param == TsParam::NoiseIndicatorThreshold) return "Noise indicator threshold";
else if (this->_param == TsParam::ScanMode) return "Scan mode";
return "Unknown parameter";
}
} // namespace toposens_driver
......@@ -3,7 +3,6 @@
* @date January 2019
*/
#include <toposens_driver/command.h>
#include "toposens_driver/sensor.h"
namespace toposens_driver
......@@ -21,7 +20,7 @@ Sensor::Sensor(ros::NodeHandle nh, ros::NodeHandle private_nh)
_serial = std::make_unique<Serial>(port);
// Set up dynamic reconfigure to change sensor settings
_srv = std::make_unique<Cfg>(private_nh);
_srv = std::make_unique<Cfg>(mutex, private_nh);
Cfg::CallbackType f = boost::bind(&Sensor::_reconfig, this, _1, _2);
_srv->setCallback(f);
......@@ -30,9 +29,10 @@ Sensor::Sensor(ros::NodeHandle nh, ros::NodeHandle private_nh)
ROS_INFO("Publishing toposens data to /%s", kScansTopic);
setMode(ScanContinuously);
_displayFirmwareVersion();
_synchronizeParamterValues();
}
/** Initalizes a specific sensor when multiple sensors are used. Gets the port and
* frame ID not from the launch parameters but as arguments of the constructor.
*/
......@@ -44,13 +44,16 @@ Sensor::Sensor(ros::NodeHandle nh, ros::NodeHandle private_nh, std::string port,
_serial = std::make_unique<Serial>(port);
// Set up dynamic reconfigure to change sensor settings
_srv = std::make_unique<Cfg>(private_nh);
_srv = std::make_unique<Cfg>(mutex, private_nh);
Cfg::CallbackType f = boost::bind(&Sensor::_reconfig, this, _1, _2);
_srv->setCallback(f);
// Publishing topic for TsScans
_pub = nh.advertise<toposens_msgs::TsScan>(kScansTopic, kQueueSize);
ROS_INFO("Publishing toposens data to /%s", kScansTopic);
_displayFirmwareVersion();
_synchronizeParamterValues();
}
......@@ -58,9 +61,37 @@ Sensor::Sensor(ros::NodeHandle nh, ros::NodeHandle private_nh, std::string port,
void Sensor::setMode(ScanMode scan_mode)
{
Command cSMode(TsParam::ScanMode, scan_mode);
if (!_serial->sendCmd(cSMode)) ROS_WARN("Sending scan mode command failed");
_serial->sendCmd(cSMode, _buffer);
if (!_evaluateAck(cSMode, _buffer.str())) ROS_WARN("Sending scan mode command failed");
}
/** Displays current firmware version of the sensor in terminal. */
void Sensor::_displayFirmwareVersion()
{
Command cFirmwareVers(TsService::FirmwareVersion);
_serial->sendCmd(cFirmwareVers, _buffer);
std::string data = _buffer.str();
size_t frame_start = data.find('S');
int ack = (data[frame_start + 6] - '0');
if (data[frame_start + 5] == '-') ack *= -1;
// The retrieving firmware version number from acknowledgement message.
try
{
if (ack == 7){
auto i = data.begin() + 8;
ROS_INFO("Firmware version: %d", (int)_toNum(i));
}
else
throw "Invalid acknowledgement error";
}
catch (...)
{
ROS_INFO("Firmware version could not be retrieved");
}
}
/** Reads datastream into a private class variable to avoid creating
* a buffer object on each poll. Assumes serial connection is alive
......@@ -105,26 +136,33 @@ void Sensor::shutdown()
*/
void Sensor::_init(void)
{
bool success = true;
Command cPulses(TsParam::NumberOfPulses, _cfg.num_pulses);
if (!_serial->sendCmd(cPulses)) success = false;
_serial->sendCmd(cPulses, _buffer);
if (!_evaluateAck(cPulses, _buffer.str())) success = false;
Command cDetect(TsParam::PeakDetectionWindow, _cfg.peak_detection_window);
if (!_serial->sendCmd(cDetect)) success = false;
_serial->sendCmd(cDetect, _buffer);
if (!_evaluateAck(cDetect, _buffer.str())) success = false;
Command cReject(TsParam::EchoRejectionThreshold, _cfg.echo_rejection_threshold);
if (!_serial->sendCmd(cReject)) success = false;
_serial->sendCmd(cReject, _buffer);
if (!_evaluateAck(cReject, _buffer.str())) success = false;
Command cNoise(TsParam::NoiseIndicatorThreshold, _cfg.noise_indicator_threshold);
if (!_serial->sendCmd(cNoise)) success = false;
_serial->sendCmd(cNoise, _buffer);
if (!_evaluateAck(cNoise, _buffer.str())) success = false;
Command cTemp(TsParam::ExternalTemperature,
_cfg.use_external_temperature ? _cfg.external_temperature : USE_INTERNAL_TEMPERATURE);
if (!_serial->sendCmd(cTemp)) success = false;
_serial->sendCmd(cTemp, _buffer);
if (!_evaluateAck(cTemp, _buffer.str())) success = false;
if (success) ROS_INFO("Sensor settings initialized");
else ROS_WARN("One or more settings failed to initialize");
}
/** Determines which setting has changed and transmits the associated
......@@ -136,44 +174,51 @@ void Sensor::_init(void)
*/
void Sensor::_reconfig(TsDriverConfig &cfg, uint32_t level)
{
if (level == 0b00000000) return;
_cfg = cfg;
if (level == -1) return Sensor::_init();
if ((int)level >= 0b10000000)
{
ROS_INFO("Update skipped: Parameter not recognized");
return;
}
_cfg = cfg;
if (level == -1) return Sensor::_init();
// Decode level Bit mask
if(level & 0b00000010)
{
Command cmd = Command(TsParam::EchoRejectionThreshold, _cfg.echo_rejection_threshold);
if (_serial->sendCmd(cmd)) ROS_INFO("Sensor setting updated");
_serial->sendCmd(cmd, _buffer);
if (_evaluateAck(cmd, _buffer.str())) ROS_INFO("Sensor setting updated");
else ROS_WARN("Settings update failed");
}
if(level & 0b00000100)
{
Command cmd = Command(TsParam::NoiseIndicatorThreshold, _cfg.noise_indicator_threshold);
if (_serial->sendCmd(cmd)) ROS_INFO("Sensor setting updated");
_serial->sendCmd(cmd, _buffer);
if (_evaluateAck(cmd, _buffer.str())) ROS_INFO("Sensor setting updated");
else ROS_WARN("Settings update failed");
}
if(level & 0b00001000)
{
Command cmd = Command(TsParam::NumberOfPulses, _cfg.num_pulses);
if (_serial->sendCmd(cmd)) ROS_INFO("Sensor setting updated");
_serial->sendCmd(cmd, _buffer);
if (_evaluateAck(cmd, _buffer.str())) ROS_INFO("Sensor setting updated");
else ROS_WARN("Settings update failed");
}
if(level & 0b00010000)
{
Command cmd = Command(TsParam::PeakDetectionWindow, _cfg.peak_detection_window);
if (_serial->sendCmd(cmd)) ROS_INFO("Sensor setting updated");
_serial->sendCmd(cmd, _buffer);
if (_evaluateAck(cmd, _buffer.str())) ROS_INFO("Sensor setting updated");
else ROS_WARN("Settings update failed");
}
......@@ -183,13 +228,13 @@ void Sensor::_reconfig(TsDriverConfig &cfg, uint32_t level)
// temperature sensor.
Command cmd = Command(TsParam::ExternalTemperature,
_cfg.use_external_temperature ? _cfg.external_temperature : USE_INTERNAL_TEMPERATURE);
if (_serial->sendCmd(cmd)) ROS_INFO("Sensor setting updated");
_serial->sendCmd(cmd, _buffer);
if (_evaluateAck(cmd, _buffer.str())) ROS_INFO("Sensor setting updated");
else ROS_WARN("Settings update failed");
}
}
/** This O(log n) algorithm only works when the input data frame is
* exactly in the expected format. Char-by-char error checks are not
* implemented so as to increase parsing throughput.
......@@ -258,8 +303,101 @@ void Sensor::_parse(const std::string &frame)
ROS_DEBUG("Error: %s in message %s", e.what(), frame.c_str());
}
}
}
/** Parses acknowledgement message into command object. */
Command* Sensor::_parseAck(const std::string &data){
size_t frame_start = data.find('S');
int ack = (data[frame_start + 6] - '0');
if (data[frame_start + 5] == '-') ack *= -1;
float val = std::atof(&data[frame_start + 8]);
return ack > 0 ? new Command((TsParam)(1 << ack), val) : nullptr;
}
/** Logs evaluation of acknowledgement message and returns true if a valid
acknowledgement message for tx_cmd was received.*/
bool Sensor::_evaluateAck(Command &tx_cmd, const std::string &data){
Command* rx_cmd = _parseAck(data);
if(rx_cmd == nullptr) return false;
// Compare parameter levels of command and acknowledgement frames.
if(tx_cmd.getParam() == rx_cmd->getParam())
{
// Compare 5 value bytes of command and acknowledgement frames.
if (strncmp(&rx_cmd->getBytes()[6], &tx_cmd.getBytes()[6], 5) == 0)
{
if (tx_cmd.getParam() != TsParam::ScanMode)
{
ROS_INFO_STREAM("TS parameter: " << tx_cmd.getParamName() << " updated to " << std::atof(&rx_cmd->getBytes()[6]));
}
return true;
}
else if(strcmp(tx_cmd.getBytes(), "CsTemp-1000\r") == 0)
{
ROS_INFO_STREAM("TS parameter: " << tx_cmd.getParamName() << " set to use internal temperature sensor.");
return true;
}
else
{
ROS_WARN_STREAM("TS parameter: " << tx_cmd.getParamName() << " clipped to " << std::atof(&rx_cmd->getBytes()[6]));
}
}
else
{
ROS_WARN_STREAM("TS parameter: " << tx_cmd.getParamName() << " value update failed!");
}
return false;
}
/** Updates according parameter value on the config server. */
void Sensor::_updateConfig(TsParam param, int value){
if (param == TsParam::NumberOfPulses) _cfg.num_pulses = value;
else if (param == TsParam::PeakDetectionWindow) _cfg.peak_detection_window = value;
else if (param == TsParam::EchoRejectionThreshold) _cfg.echo_rejection_threshold = value;
else if (param == TsParam::ExternalTemperature) _cfg.external_temperature = value;
else if (param == TsParam::NoiseIndicatorThreshold) _cfg.noise_indicator_threshold = value;
_srv->updateConfig(_cfg);
return;
}
/** Synchronizes parameter values on the config server with values used by the firmware. */
void Sensor::_synchronizeParamterValues(){
// Query current parameter configurations from TS sensor
Command cmd = Command(TsService::FirmwareConfiguration);
_serial->sendCmd(cmd, _buffer);
std::string data;
std::size_t frame_start;
int ack = 0;
while(true){
_buffer.str(std::string());
_buffer.clear();
_serial->getFrame(_buffer);
data = _buffer.str().c_str();
if(_serial->isAcknowledgementFrame(_buffer.str())){
// Parse acknowledgement message
Command* rx_cmd = _parseAck(data);
_updateConfig(rx_cmd->getParam(), rx_cmd->getValue());
}else{
// All acknowledgement messages processed.
return;
}
}
}
/** Char is a valid number if its decimal range from ASCII value '0'
* falls between 0 and 9. Number is iteratively constructed through
......
......@@ -87,7 +87,6 @@ Serial::Serial(std::string port)
// wait for 0.1s till data received
tty.c_cc[VTIME] = 1;
// Set attributes of termios structure
if(tcsetattr(_fd, TCSANOW, &tty) != 0)
{
......@@ -118,7 +117,6 @@ Serial::~Serial(void)
}
}
/** Reads incoming bytes to the string stream pointer till
* the firmware-defined frame terminator 'E' is reached.
* Returns if no data has been received for 1 second.
......@@ -147,17 +145,16 @@ void Serial::getFrame(std::stringstream &data)
if (buffer[nBytes-1] == 'E') break;
} while (ros::Time::now() - latest < ros::Duration(1));
}
}
/** Note that this returns true as long as data is written to
* the serial stream without error. A success handshake from
* the sensor confirming the settings update is not currently
* checked for.
* the serial stream without error and a response is received.
*/
bool Serial::sendCmd(Command cmd)
void Serial::sendCmd(Command cmd, std::stringstream &data)
{
char* bytes = cmd.getBytes();
data.str("");
try {
......@@ -170,87 +167,49 @@ bool Serial::sendCmd(Command cmd)
if (tx_length != -1) {
ROS_DEBUG("Bytes transmitted: %s", bytes);
return _waitForAcknowledgement(cmd);
if(!waitForAcknowledgement(data))
ROS_WARN_STREAM("Settings update timed out! - Aborting.");
} else {
ROS_ERROR("Failed to transmit %s: %s", bytes, strerror(errno));
return false;
}
}
catch (const std::exception& e)
{
ROS_ERROR("%s: %s", e.what(), cmd.getBytes());
return false;
}
}
/** Checks whether a frame is an acknowledgement for a paramter value.
*/
bool Serial::isAcknowledgementFrame(std::string frame){
size_t frame_start = frame.find('S');
if(frame_start < 0) return false;
else return (frame[frame_start + 7] == 'C');
}
/**
* Blocks execution of driver until an acknowledgement message is received or watchdog timer expires.
/** Blocks execution of driver until an acknowledgement message is received or watchdog timer expires.