Converts raw sensor data to ROS friendly message structures. More...
#include <sensor.h>
Public Types | |
| enum | ScanMode { ScanContinuously, ScanOnce, ListenOnce } |
Public Member Functions | |
| bool | poll (void) |
| Sensor (ros::NodeHandle nh, ros::NodeHandle private_nh) | |
| Sensor (ros::NodeHandle nh, ros::NodeHandle private_nh, std::string port, std::string frame_id) | |
| void | setMode (ScanMode scan_mode) |
| void | shutdown (void) |
| ~Sensor () | |
Private Types | |
| typedef dynamic_reconfigure::Server< TsDriverConfig > | Cfg |
Private Member Functions | |
| void | _displayFirmwareVersion () |
| bool | _evaluateAck (Command &cmd, const std::string &frame) |
| void | _init (void) |
| void | _parse (const std::string &frame) |
| Command * | _parseAck (const std::string &data) |
| void | _reconfig (TsDriverConfig &cfg, uint32_t level) |
| void | _synchronizeParameterValues () |
| float | _toNum (auto &i) |
| void | _updateConfig (TsParam param, int value) |
Private Attributes | |
| std::stringstream | _buffer |
| TsDriverConfig | _cfg |
| std::string | _frame_id |
| ros::Publisher | _pub |
| toposens_msgs::TsScan | _scan |
| std::unique_ptr< Serial > | _serial |
| std::unique_ptr< Cfg > | _srv |
| boost::recursive_mutex | mutex |
Converts raw sensor data to ROS friendly message structures.
Parses a TsScan from a single input data frame by extracting its header information and the vector of TsPoints contained in its payload. A TsScan contains timestamped header information followed by a vector of TsPoints. A single TsPoint has a 3D location (x, y, z) and an associated intensity. Messages are published to topic kScansTopic.
|
private |
The three different scan modes of a single sensor that are send via a command: ScanContinuously will make the sensor scan until told otherwise, ScanOnce will make the sensor scan for a single frame, ListenOnce will make the sensor listen to the echos for a single frame without sending an ultrasonic pulse first.
| Enumerator | |
|---|---|
| ScanContinuously | |
| ScanOnce | |
| ListenOnce | |
| toposens_driver::Sensor::Sensor | ( | ros::NodeHandle | nh, |
| ros::NodeHandle | private_nh | ||
| ) |
Initiates a serial connection and transmits default settings to sensor.
| nh | Public nodehandle for pub-sub ops on ROS topics. |
| private_nh | Private nodehandle for accessing launch parameters. |
A dynamic reconfigure server is set up to configure sensor performance parameters during runtime.
Definition at line 13 of file sensor.cpp.
| toposens_driver::Sensor::Sensor | ( | ros::NodeHandle | nh, |
| ros::NodeHandle | private_nh, | ||
| std::string | port, | ||
| std::string | frame_id | ||
| ) |
Is used to initialize a specific sensor when multiple sensors are used. Needs to be passed an individual private nodehandle for setting up the dynamic reconfigure server for the sensor parameters.
| nh | Public nodehandle for pub-sub ops on ROS topics. |
| private_nh | Private nodehandle for setting up the dynamic reconfigure. |
| port | Serial port that the specific sensor is connected to. |
| frame | ID of the coordinate frame the specific sensor publishes to. |
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.
Definition at line 40 of file sensor.cpp.
|
private |
Displays current firmware version of the sensor.
Displays current firmware version of the sensor in terminal.
Definition at line 70 of file sensor.cpp.
|
private |
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".
| cmd | instance of parameter update command |
| acknowledgement | message string |
Logs evaluation of acknowledgement message and returns true if a valid acknowledgement message for tx_cmd was received.
Definition at line 349 of file sensor.cpp.
|
private |
Transmits settings commands on startup with initial data from the config server.
Only parameters within the root group of cfg ParameterGenerator broadcast their default values on initialization, so this method only transmits them to the sensor.
Parameters in any sub-groups broadcast their value as 0 on startup. It is also possible that sub-group params do not broadcast their value on startup, so polling the cfg server returns 0. This is likely a bug with the ROS dynamic reconfigure library.
Definition at line 141 of file sensor.cpp.
|
private |
Extracts TsPoints from the current data frame and reads them into the referenced TsScan object.
| frame | is the data frame received. |
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.
A data frame corresponds to a single scan and has the following format:
- Starts with char 'S'
- 6 bytes of frame header info
- Char 'P', indicates a measured point
- 5 bytes of point header info
- Char 'X', indicates x-coordinate of point
- 5 bytes with measurement of x-coordinate
- Char 'Y', indicates y-coordinate of point
- 5 bytes with measurement of y-coordinate
- Char 'Z', indicates z-coordinate of point
- 5 bytes with measurement of z-coordinate
- Char 'V', indicates intensity of signal
- 5 bytes with measurement of signal intensity
- ... Additional points in this scan ...
- Ends with char 'E'
In the 6-byte long frame header:
- First byte is set to 1 when measurements are taken in a noisy ambient environment (hence, likely inaccurate).
- Second byte is currently unused.
- Third byte is set to 1 while sensor calibration is in progress.
- Fourth, fifth and sixth bytes show device address for I2C mode.
The 5-byte long pointer header is currently not used. The x-, y-, z-coordinates are calculated in mm relative to the sensor transducer. Signal intensity of the point is mapped to a scale of 0 to 255.
Sample frame: S000016P0000X-0415Y00010Z00257V00061P0000X-0235Y00019 Z00718V00055P0000X-0507Y00043Z00727V00075P0000X00142Y00360Z01555V00052E
Four points extracted: P1(-415, 10, 257, 61); P2(-235, 19, 718, 55); P3(-507, 43, 727, 75); P4(142, 360, 1555, 52)
Definition at line 290 of file sensor.cpp.
|
private |
Parses acknowledgement message into a Command object.
An acknowledgement frame corresponds to a single parameter update and has the following format:
- Starts with char 'S'
- 6 bytes of bit shifts to decode parameter level as defined in Command::Parameters, '-1' denotes unknown parameter
- Char 'C', indicates a command acknowledgement frame
- 5 bytes of firmware parameter value
- Ends with char 'E'
| acknowledgement | message string |
Parses acknowledgement message into command object.
Definition at line 335 of file sensor.cpp.
|
private |
Callback triggered when a parameter is altered on the dynamic reconfigure server. Determines which setting has changed and transmits the associated (well-formed) settings command to the serial stream.
| cfg | Structure holding updated values of all parameters on server. |
| level | Indicates parameter that triggered the callback. |
Determines which setting has changed and transmits the associated (well-formed) settings command to the serial stream. A unique level is assigned to each settings parameter in the cfg file. Current implementation defines 12 sensor performance parameters, indexed in cfg from 0 to 11. Config server triggers this method upon initialization with a special level of -1.
Definition at line 180 of file sensor.cpp.
|
private |
Synchronizes parameter values on the config server with values used by the firmware.
Definition at line 409 of file sensor.cpp.
|
private |
Efficiently converts a char array representing a signed integer to its numerical value.
| i | String iterator representing an integer value. |
| std::bad_cast | String contains non-numerical characters. |
Char is a valid number if its decimal range from ASCII value '0' falls between 0 and 9. Number is iteratively constructed through base-10 multiplication of valid digits and adding them together. The resulting number is cast to a float before returning.
Definition at line 446 of file sensor.cpp.
|
private |
Updates parameter value on the config server.
| parameter | to be updated |
| value | to update parameter within |
Updates according parameter value on the config server.
Definition at line 390 of file sensor.cpp.
| bool toposens_driver::Sensor::poll | ( | void | ) |
Retrieves raw sensor data frames and publishes TsScans extracted from them.
Reads datastream into a private class variable to avoid creating a buffer object on each poll. Assumes serial connection is alive when function is called. The high frequency at which we poll necessitates that we dispense with edge-case checks.
Definition at line 103 of file sensor.cpp.
| void toposens_driver::Sensor::setMode | ( | ScanMode | scan_mode | ) |
Sends a specific scan command to the sensor.
| the | scan command that is sent to the sensor. |
Sends a specific scan commmand to the sensor.
Definition at line 62 of file sensor.cpp.
| void toposens_driver::Sensor::shutdown | ( | void | ) |
Shuts down serial connection to the sensor.
Deletes underlying serial and config server objects managed by class pointers.
Definition at line 126 of file sensor.cpp.
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |