#include <sick_scan_common.h>
Classes | |
struct | ScanLayerFilterCfg |
Public Member Functions | |
int | ActivateStandBy (void) |
bool | changeIPandreboot (const std::string &IpAdress) |
Send a SOPAS command to the scanner that logs in the authorized client, changes the ip address and the reboots the scanner. More... | |
void | check_angle_range (SickScanConfig &conf) |
check angle setting in the config and adjust the min_ang to the max_ang if min_ang greater than max_ax More... | |
ExitCode | checkColaTypeAndSwitchToConfigured (bool useBinaryCmd) |
std::string | cmdSetAccessMode3 (void) |
Returns "sMN SetAccessMode 3 F4724744" resp. "\x02sMN SetAccessMode 3 6FD62C05\x03\0" for safety scanner. More... | |
int | convertAscii2BinaryCmd (const char *requestAscii, std::vector< unsigned char > *requestBinary) |
Convert ASCII-message to Binary-message. More... | |
virtual int | convertSendSOPASCommand (const std::string &sopas_ascii_request, std::vector< unsigned char > *reply, bool wait_for_reply=true) |
Converts a given SOPAS command from ascii to binary (in case of binary communication), sends sopas (ascii or binary) and returns the response (if wait_for_reply:=true) More... | |
std::vector< std::string > | generateExpectedAnswerString (const std::vector< unsigned char > &requestStr) |
Generate expected answer strings from the command string. More... | |
std::vector< std::string > | generateUnexpectedAnswerString (const std::string &requestStr) |
int | get2ndSopasResponse (std::vector< uint8_t > &sopas_response, const std::string &sopas_keyword) |
double | get_expected_frequency () const |
SickScanConfig * | getConfigPtr () |
ScannerBasicParam * | getCurrentParamPtr () |
uint64_t | getNanosecTimestampLastTcpMessageReceived (void) |
int | getProtocolType (void) |
get protocol type as enum More... | |
int | getReadTimeOutInMs () |
get timeout in milliseconds More... | |
bool | handleNAV350BinaryPositionData (const uint8_t *receiveBuffer, int receiveBufferLength, short &elevAngleX200, double &elevationAngleInRad, rosTime &recvTimeStamp, bool config_sw_pll_only_publish, double config_time_offset, SickGenericParser *parser_, int &numEchos, ros_sensor_msgs::LaserScan &msg, NAV350mNPOSData &navdata) |
virtual int | init (rosNodePtr nh) |
init routine of scanner More... | |
int | init_cmdTables (rosNodePtr nh) |
init command tables and define startup sequence More... | |
int | loopOnce (rosNodePtr nh) |
parsing datagram and publishing ros messages More... | |
void | messageCbNavOdomVelocity (const sick_scan_msg::NAVOdomVelocity &msg) |
int | readActiveFieldSet (int &active_field_set, std::vector< unsigned char > &sopasReply, bool useBinaryCmd=true) |
int | readFieldSetSelectionMethod (int &field_set_selection_method, std::vector< unsigned char > &sopasReply, bool useBinaryCmd=true) |
virtual bool | rebootScanner () |
Send a SOPAS command to the scanner that should cause a soft reset. More... | |
int | sendNAV350mNPOSGetData (void) |
int | sendSopasAndCheckAnswer (std::string request, std::vector< unsigned char > *reply, int cmdId) |
send command and check answer More... | |
int | sendSopasAndCheckAnswer (std::vector< unsigned char > request, std::vector< unsigned char > *reply, int cmdId) |
send command and check answer More... | |
int | sendSopasAorBgetAnswer (const std::string &request, std::vector< unsigned char > *reply, bool useBinaryCmd) |
bool | sendSopasRunSetAccessMode (bool useBinaryCmd) |
int | setAligmentMode (int _AligmentMode) |
int | setApplicationMode (bool _active, int _mode) |
void | setLengthAndCRCinBinarySopasRequest (std::vector< uint8_t > *requestBinary) |
int | setMeanFilter (bool _active, int _numberOfScans) |
int | setParticleFilter (bool _active, int _particleThreshold) |
void | setProtocolType (SopasProtocol cola_dialect_id) |
set protocol type as enum More... | |
void | setReadTimeOutInMs (int timeOutInMs) |
set timeout in milliseconds More... | |
SickScanCommon (rosNodePtr nh, SickGenericParser *parser) | |
Construction of SickScanCommon. More... | |
std::string | sopasReplyToString (const std::vector< unsigned char > &reply) |
Converts reply from sendSOPASCommand to string. More... | |
bool | testsetActivateStandBy () |
bool | testsetAligmentMode () |
bool | testsetApplicationMode () |
bool | testsetMeanFilter () |
bool | testsetParticleFilter () |
bool | testSettingIpAddress () |
void | update_config (sick_scan_xd::SickScanConfig &new_config, uint32_t level=0) |
updating configuration More... | |
int | writeActiveFieldSet (int active_field_set, std::vector< unsigned char > &sopasReply, bool useBinaryCmd=true) |
int | writeFieldSetSelectionMethod (int field_set_selection_method, std::vector< unsigned char > &sopasReply, bool useBinaryCmd=true) |
virtual | ~SickScanCommon () |
Destructor of SickScanCommon. More... | |
Static Public Member Functions | |
static bool | dumpDatagramForDebugging (unsigned char *buffer, int bufLen, bool isBinary=true) |
Public Attributes | |
ros_sensor_msgs::PointCloud2 | cloud_ |
ros_sensor_msgs::PointCloud2 | cloud_polar_ |
rosPublisher< ros_sensor_msgs::PointCloud2 > | cloud_pub_ |
SickScanConfig | config_ |
rosPublisher< sick_scan_msg::Encoder > | Encoder_pub |
rosPublisher< ros_sensor_msgs::Imu > | imuScan_pub_ |
SickScanCommonNw | m_nw |
Protected Member Functions | |
int | checkForBinaryAnswer (const std::vector< unsigned char > *reply) |
Check block for correct framing and starting sequence of a binary message. More... | |
virtual int | close_device ()=0 |
unsigned long | convertBigEndianCharArrayToUnsignedLong (const unsigned char *vecArr) |
Convert little endian to big endian (should be replaced by swap-routine) More... | |
virtual int | get_datagram (rosNodePtr nh, rosTime &recvTimeStamp, unsigned char *receiveBuffer, int bufferSize, int *actual_length, bool isBinaryProtocol, int *numberOfRemainingFifoEntries, const std::vector< std::string > &datagram_keywords)=0 |
Read a datagram from the device. More... | |
virtual int | init_device ()=0 |
virtual int | init_scanner (rosNodePtr nh) |
initialize scanner More... | |
bool | isCompatibleDevice (const std::string identStr) const |
check the identification string More... | |
int | readLIDinputstate (SickScanFieldMonSingleton *fieldMon, bool useBinaryCmd) |
virtual int | readWithTimeout (size_t timeout_ms, char *buffer, int buffer_size, int *bytes_read, const std::vector< std::string > &datagram_keywords)=0 |
std::string | replyToString (const std::vector< unsigned char > &reply) |
Converts reply from sendSOPASCommand to string. More... | |
virtual int | sendSOPASCommand (const char *request, std::vector< unsigned char > *reply, int cmdLen, bool wait_for_reply=true)=0 |
Send a SOPAS command to the device and print out the response to the console. More... | |
virtual int | stop_scanner (bool force_immediate_shutdown=false) |
Stops sending scan data. More... | |
bool | switchColaProtocol (bool useBinaryCmd) |
Private Member Functions | |
bool | checkForProtocolChangeAndMaybeReconnect (bool &useBinaryCmdNow) |
checks the current protocol type and gives information about necessary change More... | |
bool | getSensorIsRadar (void) |
int | readParseSafetyFields (bool useBinaryCmd) |
bool | setNewIpAddress (const std::string &ipNewIPAddr, bool useBinaryCmd) |
bool | setNTPServerAndStart (const std::string &ipNewIPAddr, bool useBinaryCmd) |
void | setSensorIsRadar (bool _isRadar) |
Definition at line 102 of file sick_scan_common.h.
Definition at line 105 of file sick_scan_common.h.
sick_scan_xd::SickScanCommon::SickScanCommon | ( | rosNodePtr | nh, |
SickGenericParser * | parser | ||
) |
Construction of SickScanCommon.
parser | Corresponding parser holding specific scanner parameter |
Definition at line 462 of file sick_scan_common.cpp.
|
virtual |
Destructor of SickScanCommon.
Definition at line 919 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::ActivateStandBy | ( | void | ) |
bool sick_scan_xd::SickScanCommon::changeIPandreboot | ( | const std::string & | IpAdress | ) |
Send a SOPAS command to the scanner that logs in the authorized client, changes the ip address and the reboots the scanner.
IpAdress | new IP address |
void sick_scan_xd::SickScanCommon::check_angle_range | ( | SickScanConfig & | conf | ) |
check angle setting in the config and adjust the min_ang to the max_ang if min_ang greater than max_ax
Definition at line 5384 of file sick_scan_common.cpp.
ExitCode sick_scan_xd::SickScanCommon::checkColaTypeAndSwitchToConfigured | ( | bool | useBinaryCmd | ) |
Definition at line 1289 of file sick_scan_common.cpp.
|
protected |
Check block for correct framing and starting sequence of a binary message.
[in] | reply | check reply whether is SOPAS-ASCII or SOPAS-Binary |
reply | Pointer to datablock |
Definition at line 793 of file sick_scan_common.cpp.
|
private |
checks the current protocol type and gives information about necessary change
useBinaryCmdNow | Input/Output: Holds information about current protocol |
Definition at line 6059 of file sick_scan_common.cpp.
|
protectedpure virtual |
Implemented in sick_scan_xd::SickScanCommonTcp.
std::string sick_scan_xd::SickScanCommon::cmdSetAccessMode3 | ( | void | ) |
Returns "sMN SetAccessMode 3 F4724744" resp. "\x02sMN SetAccessMode 3 6FD62C05\x03\0" for safety scanner.
Definition at line 715 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::convertAscii2BinaryCmd | ( | const char * | requestAscii, |
std::vector< unsigned char > * | requestBinary | ||
) |
Convert ASCII-message to Binary-message.
requestAscii | holds ASCII-encoded command |
requestBinary | hold binary command as vector of unsigned char |
Definition at line 5455 of file sick_scan_common.cpp.
|
protected |
Convert little endian to big endian (should be replaced by swap-routine)
[in] | *vecArr | to (unsigned) char buffer in big endian byte oder (MSB first) |
*vecArr | Pointer to 4 byte block |
Definition at line 776 of file sick_scan_common.cpp.
|
virtual |
Converts a given SOPAS command from ascii to binary (in case of binary communication), sends sopas (ascii or binary) and returns the response (if wait_for_reply:=true)
[in] | request | the command to send. |
[in] | cmdLen | Length of the Comandstring in bytes used for Binary Mode only |
Definition at line 830 of file sick_scan_common.cpp.
|
static |
Definition at line 4233 of file sick_scan_common.cpp.
std::vector< std::string > sick_scan_xd::SickScanCommon::generateExpectedAnswerString | ( | const std::vector< unsigned char > & | requestStr | ) |
Generate expected answer strings from the command string.
Changes the Identifier of a commandstr. to its expected answer counterpart
requestStr | sent request string |
requestStr | command string (either as ASCII or BINARY) |
Map that defines expected answer identifiers
Definition at line 932 of file sick_scan_common.cpp.
std::vector< std::string > sick_scan_xd::SickScanCommon::generateUnexpectedAnswerString | ( | const std::string & | requestStr | ) |
Returns a list of unexpected lidar responses like "" for request ""
requestStr | sent request string |
Definition at line 1050 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::get2ndSopasResponse | ( | std::vector< uint8_t > & | sopas_response, |
const std::string & | sopas_keyword | ||
) |
Definition at line 1252 of file sick_scan_common.cpp.
|
protectedpure virtual |
Read a datagram from the device.
[out] | recvTimeStamp | timestamp of received packet |
[in] | receiveBuffer | data buffer to fill |
[in] | bufferSize | max data size to write to buffer (result should be 0 terminated) |
[out] | actual_length | the actual amount of data written |
[in] | isBinaryProtocol | used Communication protocol True=Binary false=ASCII |
[in] | datagram_keywords | keyword in returned datagram, e.g. { "LMDscandata" } to get scandata telegrams, or {} (empty vector) for next received datagram |
Implemented in sick_scan_xd::SickScanCommonTcp.
|
inline |
Definition at line 319 of file sick_scan_common.h.
|
inline |
Definition at line 343 of file sick_scan_common.h.
|
inline |
Definition at line 348 of file sick_scan_common.h.
|
inline |
Definition at line 365 of file sick_scan_common.h.
int sick_scan_xd::SickScanCommon::getProtocolType | ( | void | ) |
get protocol type as enum
Definition at line 1483 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::getReadTimeOutInMs | ( | ) |
get timeout in milliseconds
Definition at line 1473 of file sick_scan_common.cpp.
|
private |
Definition at line 6218 of file sick_scan_common.cpp.
bool sick_scan_xd::SickScanCommon::handleNAV350BinaryPositionData | ( | const uint8_t * | receiveBuffer, |
int | receiveBufferLength, | ||
short & | elevAngleX200, | ||
double & | elevationAngleInRad, | ||
rosTime & | recvTimeStamp, | ||
bool | config_sw_pll_only_publish, | ||
double | config_time_offset, | ||
SickGenericParser * | parser_, | ||
int & | numEchos, | ||
ros_sensor_msgs::LaserScan & | msg, | ||
NAV350mNPOSData & | navdata | ||
) |
Definition at line 1376 of file sick_scan_common.cpp.
|
virtual |
int sick_scan_xd::SickScanCommon::init_cmdTables | ( | rosNodePtr | nh | ) |
init command tables and define startup sequence
Definition at line 1541 of file sick_scan_common.cpp.
|
protectedpure virtual |
Implemented in sick_scan_xd::SickScanCommonTcp.
|
protectedvirtual |
|
protected |
check the identification string
identStr | string (got from sopas request) |
Definition at line 4277 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::loopOnce | ( | rosNodePtr | nh | ) |
parsing datagram and publishing ros messages
Definition at line 4360 of file sick_scan_common.cpp.
void sick_scan_xd::SickScanCommon::messageCbNavOdomVelocity | ( | const sick_scan_msg::NAVOdomVelocity & | msg | ) |
Definition at line 1422 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::readActiveFieldSet | ( | int & | active_field_set, |
std::vector< unsigned char > & | sopasReply, | ||
bool | useBinaryCmd = true |
||
) |
Definition at line 6461 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::readFieldSetSelectionMethod | ( | int & | field_set_selection_method, |
std::vector< unsigned char > & | sopasReply, | ||
bool | useBinaryCmd = true |
||
) |
Definition at line 6409 of file sick_scan_common.cpp.
|
protected |
Definition at line 6358 of file sick_scan_common.cpp.
|
private |
Definition at line 6270 of file sick_scan_common.cpp.
|
protectedpure virtual |
Implemented in sick_scan_xd::SickScanCommonTcp.
|
virtual |
Send a SOPAS command to the scanner that should cause a soft reset.
Reboot scanner.
Definition at line 853 of file sick_scan_common.cpp.
|
protected |
Converts reply from sendSOPASCommand to string.
convert ASCII or binary reply to a human readable string
[in] | reply | reply from sendSOPASCommand |
reply | datablock, which should be converted |
Definition at line 4190 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::sendNAV350mNPOSGetData | ( | void | ) |
Definition at line 1364 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::sendSopasAndCheckAnswer | ( | std::string | requestStr, |
std::vector< unsigned char > * | reply, | ||
int | cmdId = -1 |
||
) |
send command and check answer
requestStr | Sopas-Command |
*reply | Antwort-String |
cmdId | Command index to derive the correct error message (optional) |
Definition at line 1068 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::sendSopasAndCheckAnswer | ( | std::vector< unsigned char > | requestStr, |
std::vector< unsigned char > * | reply, | ||
int | cmdId = -1 |
||
) |
send command and check answer
requestStr | Sopas-Command given as byte-vector |
*reply | Antwort-String |
cmdId | Command index to derive the correct error message (optional) |
Definition at line 1086 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::sendSopasAorBgetAnswer | ( | const std::string & | request, |
std::vector< unsigned char > * | reply, | ||
bool | useBinaryCmd | ||
) |
Definition at line 1227 of file sick_scan_common.cpp.
|
protectedpure virtual |
Send a SOPAS command to the device and print out the response to the console.
[in] | request | the command to send. |
[out] | reply | if not NULL, will be filled with the reply package to the command. |
[in] | cmdLen | Length of the Comandstring in bytes used for Binary Mode only |
Implemented in sick_scan_xd::SickScanCommonTcp.
bool sick_scan_xd::SickScanCommon::sendSopasRunSetAccessMode | ( | bool | useBinaryCmd | ) |
Definition at line 1337 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::setAligmentMode | ( | int | _AligmentMode | ) |
int sick_scan_xd::SickScanCommon::setApplicationMode | ( | bool | _active, |
int | _mode | ||
) |
void sick_scan_xd::SickScanCommon::setLengthAndCRCinBinarySopasRequest | ( | std::vector< uint8_t > * | requestBinary | ) |
Definition at line 6031 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::setMeanFilter | ( | bool | _active, |
int | _numberOfScans | ||
) |
|
private |
Definition at line 6095 of file sick_scan_common.cpp.
|
private |
Definition at line 6154 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::setParticleFilter | ( | bool | _active, |
int | _particleThreshold | ||
) |
void sick_scan_xd::SickScanCommon::setProtocolType | ( | SopasProtocol | cola_dialect_id | ) |
set protocol type as enum
Definition at line 1492 of file sick_scan_common.cpp.
void sick_scan_xd::SickScanCommon::setReadTimeOutInMs | ( | int | timeOutInMs | ) |
set timeout in milliseconds
timeOutInMs | in milliseconds |
Definition at line 1463 of file sick_scan_common.cpp.
|
private |
Definition at line 6213 of file sick_scan_common.cpp.
|
inline |
Converts reply from sendSOPASCommand to string.
[in] | reply | reply from sendSOPASCommand |
Definition at line 360 of file sick_scan_common.h.
|
protectedvirtual |
|
protected |
Definition at line 1267 of file sick_scan_common.cpp.
bool sick_scan_xd::SickScanCommon::testsetActivateStandBy | ( | ) |
bool sick_scan_xd::SickScanCommon::testsetAligmentMode | ( | ) |
bool sick_scan_xd::SickScanCommon::testsetApplicationMode | ( | ) |
bool sick_scan_xd::SickScanCommon::testsetMeanFilter | ( | ) |
bool sick_scan_xd::SickScanCommon::testsetParticleFilter | ( | ) |
bool sick_scan_xd::SickScanCommon::testSettingIpAddress | ( | ) |
void sick_scan_xd::SickScanCommon::update_config | ( | sick_scan_xd::SickScanConfig & | new_config, |
uint32_t | level = 0 |
||
) |
updating configuration
new_config | Pointer to new configuration |
level | (not used - should be removed) |
Definition at line 5398 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::writeActiveFieldSet | ( | int | active_field_set, |
std::vector< unsigned char > & | sopasReply, | ||
bool | useBinaryCmd = true |
||
) |
Definition at line 6438 of file sick_scan_common.cpp.
int sick_scan_xd::SickScanCommon::writeFieldSetSelectionMethod | ( | int | field_set_selection_method, |
std::vector< unsigned char > & | sopasReply, | ||
bool | useBinaryCmd = true |
||
) |
Definition at line 6386 of file sick_scan_common.cpp.
|
private |
Definition at line 550 of file sick_scan_common.h.
ros_sensor_msgs::PointCloud2 sick_scan_xd::SickScanCommon::cloud_ |
Definition at line 386 of file sick_scan_common.h.
|
private |
Definition at line 477 of file sick_scan_common.h.
ros_sensor_msgs::PointCloud2 sick_scan_xd::SickScanCommon::cloud_polar_ |
Definition at line 387 of file sick_scan_common.h.
rosPublisher<ros_sensor_msgs::PointCloud2> sick_scan_xd::SickScanCommon::cloud_pub_ |
Definition at line 383 of file sick_scan_common.h.
|
private |
Definition at line 465 of file sick_scan_common.h.
SickScanConfig sick_scan_xd::SickScanCommon::config_ |
Definition at line 395 of file sick_scan_common.h.
|
private |
Definition at line 468 of file sick_scan_common.h.
|
private |
Definition at line 514 of file sick_scan_common.h.
|
private |
Definition at line 511 of file sick_scan_common.h.
rosPublisher<sick_scan_msg::Encoder> sick_scan_xd::SickScanCommon::Encoder_pub |
Definition at line 385 of file sick_scan_common.h.
|
private |
Definition at line 513 of file sick_scan_common.h.
rosPublisher<ros_sensor_msgs::Imu> sick_scan_xd::SickScanCommon::imuScan_pub_ |
Definition at line 384 of file sick_scan_common.h.
|
private |
Definition at line 471 of file sick_scan_common.h.
|
private |
Definition at line 473 of file sick_scan_common.h.
|
private |
Definition at line 474 of file sick_scan_common.h.
|
private |
Definition at line 560 of file sick_scan_common.h.
|
private |
Definition at line 552 of file sick_scan_common.h.
|
private |
Definition at line 580 of file sick_scan_common.h.
SickScanCommonNw sick_scan_xd::SickScanCommon::m_nw |
Definition at line 341 of file sick_scan_common.h.
|
private |
Definition at line 464 of file sick_scan_common.h.
|
private |
Definition at line 557 of file sick_scan_common.h.
|
private |
Definition at line 558 of file sick_scan_common.h.
|
private |
Definition at line 578 of file sick_scan_common.h.
|
private |
Definition at line 481 of file sick_scan_common.h.
|
private |
Definition at line 479 of file sick_scan_common.h.
|
private |
Definition at line 482 of file sick_scan_common.h.
|
private |
Definition at line 486 of file sick_scan_common.h.
|
private |
Definition at line 485 of file sick_scan_common.h.
|
private |
Definition at line 529 of file sick_scan_common.h.
|
private |
Definition at line 520 of file sick_scan_common.h.
|
private |
Definition at line 467 of file sick_scan_common.h.
|
private |
Definition at line 469 of file sick_scan_common.h.
|
private |
Definition at line 472 of file sick_scan_common.h.
|
private |
Definition at line 475 of file sick_scan_common.h.
|
private |
Definition at line 476 of file sick_scan_common.h.
|
private |
Definition at line 483 of file sick_scan_common.h.
|
private |
Definition at line 480 of file sick_scan_common.h.
|
private |
Definition at line 543 of file sick_scan_common.h.
|
private |
Definition at line 548 of file sick_scan_common.h.
|
private |
Definition at line 527 of file sick_scan_common.h.
|
private |
Definition at line 526 of file sick_scan_common.h.
|
private |
Definition at line 522 of file sick_scan_common.h.
|
private |
Definition at line 521 of file sick_scan_common.h.
|
private |
Definition at line 524 of file sick_scan_common.h.
|
private |
Definition at line 525 of file sick_scan_common.h.
|
private |
Definition at line 523 of file sick_scan_common.h.
|
private |
Definition at line 545 of file sick_scan_common.h.