Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
What's new
9
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Switch to GitLab Next
Sign in / Register
Toggle navigation
Open sidebar
Kyle Niemeyer
ros-packages
Commits
e3406fd9
Commit
e3406fd9
authored
Jul 18, 2019
by
Christopher Lang
Browse files
Options
Browse Files
Download
Plain Diff
Merge branch 'sync_paramter_states' into 'master'
Sync paramter states See merge request
toposens/public/ros-packages!78
parents
f4598ae4
69c8d163
Changes
12
Hide whitespace changes
Inline
Side-by-side
Showing
12 changed files
with
360 additions
and
177 deletions
+360
-177
toposens/package.xml
toposens/package.xml
+1
-1
toposens_driver/include/toposens_driver/command.h
toposens_driver/include/toposens_driver/command.h
+26
-2
toposens_driver/include/toposens_driver/sensor.h
toposens_driver/include/toposens_driver/sensor.h
+44
-0
toposens_driver/include/toposens_driver/serial.h
toposens_driver/include/toposens_driver/serial.h
+19
-12
toposens_driver/src/lib/command.cpp
toposens_driver/src/lib/command.cpp
+29
-3
toposens_driver/src/lib/sensor.cpp
toposens_driver/src/lib/sensor.cpp
+159
-21
toposens_driver/src/lib/serial.cpp
toposens_driver/src/lib/serial.cpp
+21
-62
toposens_driver/tests/modules/reconfig/tests.cpp
toposens_driver/tests/modules/reconfig/tests.cpp
+20
-44
toposens_driver/tests/modules/sensor/tests.cpp
toposens_driver/tests/modules/sensor/tests.cpp
+22
-14
toposens_markers/package.xml
toposens_markers/package.xml
+1
-1
toposens_sync/package.xml
toposens_sync/package.xml
+1
-1
toposens_sync/tests/modules/sensor_manager/tests.cpp
toposens_sync/tests/modules/sensor_manager/tests.cpp
+17
-16
No files found.
toposens/package.xml
View file @
e3406fd9
...
...
@@ -24,7 +24,7 @@
<exec_depend>
toposens_pointcloud
</exec_depend>
<exec_depend>
toposens_description
</exec_depend>
<exec_depend>
toposens_sync
</exec_depend>
<export>
<metapackage/>
</export>
...
...
toposens_driver/include/toposens_driver/command.h
View file @
e3406fd9
...
...
@@ -20,7 +20,7 @@ namespace toposens_driver
* concatenated by an OR operation.
*/
enum
TsParam
{
{
UseExternalTemperature
=
0b00000001
,
/**< Switch between internal temperature sensor value or user-defined temp value. */
EchoRejectionThreshold
=
0b00000010
,
/**< Minimum amplitude for an echo to be considered valid [0 to 20]. */
NoiseIndicatorThreshold
=
0b00000100
,
/**< Normalized noise level on ADC signals to mark processed points as noisy. */
...
...
@@ -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
...
...
toposens_driver/include/toposens_driver/sensor.h
View file @
e3406fd9
...
...
@@ -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.*/
...
...
toposens_driver/include/toposens_driver/serial.h
View file @
e3406fd9
...
...
@@ -34,21 +34,23 @@ class Serial
void
getFrame
(
std
::
stringstream
&
data
);
/** Writes the given bytes to the serial stream and confirms a parameter update.
* Usually used for updating sensor settings during runtime.
* Usually used for updating sensor settings during runtime.
* @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 otherwis
e
* @param
frame string received from sensor
* @return true if acknowledgement
is received, false if watchdog terminated blocking mod
e
*/
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.*/
};
...
...
toposens_driver/src/lib/command.cpp
View file @
e3406fd9
...
...
@@ -12,7 +12,7 @@ namespace toposens_driver
* The first byte is always reserved for the arithmetic sign: 0 for
* positive values, - for negative values. The function also clips
* desired parameter values to predefined bounds.
*
*
* Command format:
* @n - Starts with #kPrefix
* @n - 5 bytes defining firmware parameter to update
...
...
@@ -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
));
}
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
"
;
_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
toposens_driver/src/lib/sensor.cpp
View file @
e3406fd9
...
...
@@ -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
...
...
toposens_driver/src/lib/serial.cpp
View file @
e3406fd9
...
...
@@ -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