Skip to content
This repository has been archived by the owner on Mar 8, 2023. It is now read-only.

Commit

Permalink
Release 1.8.2, activated OUT4
Browse files Browse the repository at this point in the history
  • Loading branch information
@rostest
rostest committed Feb 4, 2021
1 parent 40d3205 commit d3634ac
Show file tree
Hide file tree
Showing 10 changed files with 105 additions and 49 deletions.
2 changes: 1 addition & 1 deletion cfg/SickScan.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,5 +74,5 @@ gen.add("scan_freq", double_t, 0, "Scan frequency set to 0 to use scanner defaul
gen.add("encoder_mode", int_t, 0, "-1:No Encoder, 0:Off, 1:Single increment, 2:Direction Phase, 3:Direction Level",-1 ,-1,3)
# gen.add("mean_filter", int_t, 0, "Number of averages for mean filter 0 means filter is disabled", 0, 0, 100)
# gen.add("mirror_scan",bool_t, 0, "Scan direction's changed. E.g. for overhead mounting or NAV 310 ( in contrast to other sick scanners NAV 310 is clockwise rotating ).",False)
gen.add("use_safty_fields", bool_t, 0, "Whether or not to use safty fields. Only tim 7xx5 supported at the moment", True)
# gen.add("use_safety_fields", bool_t, 0, "Whether or not to use safety fields. Only tim 7xxS supported at the moment", False) # True)
exit(gen.generate(PACKAGE, "sick_scan", "SickScan"))
Binary file modified doc/tim7xxs_screenshot01.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified doc/tim7xxs_screenshot02.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added doc/tim7xxs_screenshot03.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
39 changes: 21 additions & 18 deletions driver/src/sick_generic_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -332,18 +332,21 @@ namespace sick_scan
return (useSafetyPasWD);
}

bool ScannerBasicParam::getUseSaftyFields(){
return(useSaftyFields);
bool ScannerBasicParam::getUseSafetyFields(){
return(useSafetyFields);
}

void ScannerBasicParam::setUseSaftyFields(bool _useSaftyFields){
this->useSaftyFields=_useSaftyFields;
void ScannerBasicParam::setUseSafetyFields(bool _useSafetyFields){
this->useSafetyFields=_useSafetyFields;
}
/*!
\brief Construction of parameter object
*/
ScannerBasicParam::ScannerBasicParam()
: numberOfLayers(0), numberOfShots(0), numberOfMaximumEchos(0), elevationDegreeResolution(0), angleDegressResolution(0), expectedFrequency(0),
useBinaryProtocol(false), IntensityResolutionIs16Bit(false), deviceIsRadar(false), useSafetyPasWD(false), encoderMode(0),
CartographerCompatibility(false), scanMirroredAndShifted(false), useSafetyFields(false)
{
this->elevationDegreeResolution = 0.0;
this->setUseBinaryProtocol(false);
Expand Down Expand Up @@ -417,7 +420,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);
}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) ==
0) // LMS1000 - 4 layer, 1101 shots per scan
Expand All @@ -433,7 +436,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);
}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_240_NAME) ==
0) // TIM_5xx - 1 Layer, max. 811 shots per scan
Expand All @@ -448,7 +451,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);

}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_5XX_NAME) ==
Expand All @@ -464,7 +467,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);

}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_4XXX_NAME) == 0) // LMS_4xxx - 1 Layer, 600 Hz
Expand All @@ -479,7 +482,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);
}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XX_NAME) == 0) // TIM_7xx - 1 Layer Scanner
{
Expand All @@ -493,7 +496,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);
}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XXS_NAME) == 0) // TIM_7xxS - 1 layer Safety Scanner
{
Expand All @@ -507,7 +510,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(true); // Safety scanner
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(true);
basicParams[i].setUseSafetyFields(true);
}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0) // LMS_5xx - 1 Layer
{
Expand All @@ -521,7 +524,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);
}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XX_NAME) == 0) // LMS_1xx - 1 Layer
{
Expand All @@ -535,7 +538,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);
}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_MRS_6XXX_NAME) == 0) //
{
Expand All @@ -550,7 +553,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);
}

if (basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_3XX_NAME) == 0) // Radar
Expand All @@ -566,7 +569,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);
}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) // Nav 3xx
{
Expand All @@ -578,7 +581,7 @@ namespace sick_scan
basicParams[i].setUseBinaryProtocol(true);
basicParams[i].setDeviceIsRadar(false); // Default
basicParams[i].setScanMirroredAndShifted(true);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);
}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_2XX_NAME) == 0) // NAV_2xx - 1 Layer
{
Expand All @@ -592,7 +595,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);
}
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_4XX_NAME) == 0) // TiM433 and TiM443
{
Expand All @@ -606,7 +609,7 @@ namespace sick_scan
basicParams[i].setUseSafetyPasWD(false); // Default
basicParams[i].setEncoderMode(-1); // Default
basicParams[i].setScanMirroredAndShifted(false);
basicParams[i].setUseSaftyFields(false);
basicParams[i].setUseSafetyFields(false);
}
}

Expand Down
8 changes: 4 additions & 4 deletions driver/src/sick_scan_common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1170,13 +1170,13 @@ namespace sick_scan

bool rssiFlag = false;
bool rssiResolutionIs16Bit = true; //True=16 bit Flase=8bit
bool useSaftyields=false;
// bool useSafetyfields=false;
int activeEchos = 0;
ros::NodeHandle pn("~");

pn.getParam("intensity", rssiFlag);
pn.getParam("intensity_resolution_16bit", rssiResolutionIs16Bit);
pn.getParam("use_safty_fields", useSaftyields);
// pn.getParam("use_safety_fields", useSafetyfields);
//check new ip adress and add cmds to write ip to comand chain
std::string sNewIPAddr = "";
boost::asio::ip::address_v4 ipNewIPAddr;
Expand Down Expand Up @@ -1215,7 +1215,7 @@ namespace sick_scan
}

this->parser_->getCurrentParamPtr()->setIntensityResolutionIs16Bit(rssiResolutionIs16Bit);
this->parser_->getCurrentParamPtr()->setUseSaftyFields(useSaftyields);
// this->parser_->getCurrentParamPtr()->setUseSafetyFields(useSafetyfields);
// parse active_echos entry and set flag array
pn.getParam("active_echos", activeEchos);

Expand Down Expand Up @@ -2106,7 +2106,7 @@ namespace sick_scan

}
//SAFTY FIELD PARSING
if (this->parser_->getCurrentParamPtr()->getUseSaftyFields())
if (this->parser_->getCurrentParamPtr()->getUseSafetyFields())
{
ROS_INFO("Reading safety fields");
SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance();
Expand Down
84 changes: 68 additions & 16 deletions driver/src/sick_scan_messages.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,15 +85,44 @@ bool sick_scan::SickScanMessages::parseLIDoutputstateMsg(const ros::Time& timeSt
if(useBinaryProtocol)
{
// parse and convert LIDoutputstate message
int dbg_telegram_length = receiveLength;
std::string dbg_telegram_hex_copy = DataDumper::binDataToAsciiString(receiveBuffer, receiveLength);
int msg_start_idx = 27; // 4 byte STX + 4 byte payload_length + 19 byte "sSN LIDoutputstate " = 27 byte
int msg_parameter_length = receiveLength - msg_start_idx - 1; // start bytes + 1 byte CRC
int msg_output_states_length = msg_parameter_length - 18; // 2 byte version + 4 byte system + 12 byte timestamp = 18 byte
int number_of_output_states = msg_output_states_length / 5; // each output state has 1 byte state enum + 4 byte counter = 5 byte
if((msg_output_states_length % 5) != 0 || number_of_output_states <= 0)
if(msg_parameter_length < 8) // at least 6 byte (version+system) + 2 byte (timestamp status)
{
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): received " << receiveLength << " byte with " << msg_output_states_length << " byte states, expected multiple of 5 (" << __FILE__ << ":" << __LINE__ << ")");
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): received " << receiveLength << " byte, expected at least 8 byte (" << __FILE__ << ":" << __LINE__ << ")");
return false;
}
// The LIDoutputstate parameter may have 11 byte timestamp or not, i.e. the parameter format is either:
// a) msg_parameter_length := (6 byte version+system) + number_of_output_states * (5 byte state+counter per output_state) + (2 byte 0x0000 no timestamp), or
// b) msg_parameter_length := (6 byte version+system) + number_of_output_states * (5 byte state+counter per output_state) + (2 byte 0x0001 timestamp available) + (11 byte timestamp)
bool parameter_format_ok = false;
// Check parameter format a (default, TiM781S: no timestamp)
uint16_t timestamp_status = (receiveBuffer[receiveLength - 3] << 8) | (receiveBuffer[receiveLength - 2]); // 2 byte before receiveBuffer[receiveLength-1] = CRC
int number_of_output_states = (msg_parameter_length - 6 - 2) / 5; // each output state has 1 byte state enum + 4 byte counter = 5 byte
if(msg_parameter_length == 6 + number_of_output_states * 5 + 2 && timestamp_status == 0)
{
parameter_format_ok = true;
ROS_DEBUG_STREAM("SickScanMessages::parseLIDoutputstateMsg(): " << number_of_output_states << " output states, no timestamp" );
}
else // Check parameter format b with timestamp
{
timestamp_status = (receiveBuffer[receiveLength - 14] << 8) | (receiveBuffer[receiveLength - 13]); // 2 byte before receiveBuffer[receiveLength-12] = (11 byte timestamp) + (1 byte CRC)
number_of_output_states = (msg_parameter_length - 6 - 2 - 11) / 5; // each output state has 1 byte state enum + 4 byte counter = 5 byte
if(msg_parameter_length == 6 + number_of_output_states * 5 + 2 + 11 && timestamp_status > 0)
{
parameter_format_ok = true;
ROS_DEBUG_STREAM("SickScanMessages::parseLIDoutputstateMsg(): " << number_of_output_states << " output states and timestamp" );
}
}
if(!parameter_format_ok)
{
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): received " << receiveLength << " byte with invalid format (" << __FILE__ << ":" << __LINE__ << "): "
<< DataDumper::binDataToAsciiString(receiveBuffer, receiveLength));
return false;
}
// Read 2 byte version + 2 byte system status
receiveBuffer += msg_start_idx;
receiveLength -= msg_start_idx;
if( !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.version_number)
Expand All @@ -102,6 +131,7 @@ bool sick_scan::SickScanMessages::parseLIDoutputstateMsg(const ros::Time& timeSt
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing version_number and system_counter (" << __FILE__ << ":" << __LINE__ << ")");
return false;
}
// Read N output states
output_msg.output_state.reserve(number_of_output_states);
output_msg.output_count.reserve(number_of_output_states);
for(int state_cnt = 0; state_cnt < number_of_output_states; state_cnt++)
Expand All @@ -114,29 +144,51 @@ bool sick_scan::SickScanMessages::parseLIDoutputstateMsg(const ros::Time& timeSt
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing version_number and system_counter (" << __FILE__ << ":" << __LINE__ << ")");
return false;
}
output_msg.output_state.push_back(output_state);
output_msg.output_count.push_back(output_count);
if(output_state == 0 || output_state == 1) // 0: not active, 1: active, 2: not used
{
output_msg.output_state.push_back(output_state);
output_msg.output_count.push_back(output_count);
}
}
if( !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.time_state)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.year)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.month)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.day)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.hour)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.minute)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.second)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.microsecond))
// Read timestamp state
if( !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.time_state))
{
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing timestamp (" << __FILE__ << ":" << __LINE__ << ")");
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing time_state (" << __FILE__ << ":" << __LINE__ << ")");
return false;
}
if(output_msg.time_state != timestamp_status) // time_state and previously parsed timestamp_status must be identical
{
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): time_state mismatch, received " << (int)output_msg.time_state << ", expected " << (int)timestamp_status << " (" << __FILE__ << ":" << __LINE__ << ")");
return false;

}
// Read optional timestamp
if(timestamp_status > 0)
{
if(!readBinaryBuffer(receiveBuffer, receiveLength, output_msg.year)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.month)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.day)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.hour)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.minute)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.second)
|| !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.microsecond))
{
ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing timestamp (" << __FILE__ << ":" << __LINE__ << ")");
return false;
}
}
output_msg.header.stamp = timeStamp;
output_msg.header.seq = 0;
output_msg.header.frame_id = frame_id;

// Debug messages
std::stringstream state_str;
for(int state_cnt = 0; state_cnt < number_of_output_states; state_cnt++)
assert(output_msg.output_state.size() == output_msg.output_state.size());
state_str << number_of_output_states << " output states received" << (timestamp_status?" with":", no") << " timestamp\n";
for(int state_cnt = 0; state_cnt < output_msg.output_count.size(); state_cnt++)
state_str << "state[" << state_cnt << "]: " << (uint32_t)output_msg.output_state[state_cnt] << ", count=" << (uint32_t)output_msg.output_count[state_cnt] << "\n";
ROS_DEBUG_STREAM("SickScanMessages::parseLIDoutputstateMsg():\n"
<< dbg_telegram_length << " byte telegram: " << dbg_telegram_hex_copy << "\n"
<< "version_number: " << (uint32_t)output_msg.version_number << ", system_counter: " << (uint32_t)output_msg.system_counter << "\n"
<< state_str.str()
<< "time state: " << (uint32_t)output_msg.time_state
Expand Down
6 changes: 3 additions & 3 deletions include/sick_scan/sick_generic_parser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,9 +115,9 @@ namespace sick_scan

int8_t getEncoderMode();

bool getUseSaftyFields();
bool getUseSafetyFields();

void setUseSaftyFields(bool _useSaftyFields);
void setUseSafetyFields(bool _useSafetyFields);

private:
std::string scannerName;
Expand All @@ -134,7 +134,7 @@ namespace sick_scan
int8_t encoderMode;
bool CartographerCompatibility;
bool scanMirroredAndShifted;
bool useSaftyFields;
bool useSafetyFields;
};


Expand Down
2 changes: 1 addition & 1 deletion msg/LIDoutputstateMsg.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ uint8[] output_state # output states, each state with value 0 (not active), 1 (
uint32[] output_count # output counter

# Time block (sensortime from the last change of min. one of the outputs)
uint8 time_state # No time data: 0, Time data: 1
uint16 time_state # No time data: 0, Time data: 1
uint16 year # f.e. 2021
uint8 month # 1 ... 12
uint8 day # 1 ... 31
Expand Down
Loading

0 comments on commit d3634ac

Please sign in to comment.