Skip to content

Commit

Permalink
Release v3.1.0
Browse files Browse the repository at this point in the history
  * add: IMU support for multiScan
  * add: LaserScan output for picoScan
  * fix: API reinit 
  * fix: multiScan data output with range filter activated
  * fix: adapt multiScan startup and shutdown sequence
  * fix: adaptations for MRS-1000 v2 firmware
  • Loading branch information
aiplemaSICKAG committed Nov 21, 2023
1 parent 7b6fe84 commit e5f7206
Show file tree
Hide file tree
Showing 99 changed files with 22,253 additions and 879 deletions.
9 changes: 9 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,15 @@ features that will be removed in future versions **Removed** for deprecated feat

## Released ##

### Release v3.1.0

- **add** IMU support for multiScan
- **add** LaserScan output for picoScan
- **fix** API reinit
- **fix** multiScan data output with range filter activated
- **fix** adapt multiScan startup and shutdown sequence
- **fix** adaptations for MRS-1000 v2 firmware

### Release v3.0.0

This release has a new major version as it breaks with the previously used ROS module name "sick_scan".
Expand Down
20 changes: 3 additions & 17 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -371,23 +371,9 @@ endif()
if(${BUILD_WITH_SCANSEGMENT_XD_SUPPORT} OR BUILD_WITH_SCANSEGMENT_XD_SUPPORT EQUAL ON)
message(STATUS "Building ${PROJECT_NAME} with SCANSEGMENT_XD support")
add_compile_options(-DSCANSEGMENT_XD_SUPPORT=1)
# msgpack11
if(WIN32 OR ROS_VERSION EQUAL 0)
set(MSGPACK11_INCLUDE_DIR "${PROJECT_SOURCE_DIR}/../msgpack11")
else()
set(MSGPACK11_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/../msgpack11/include")
endif()
if(WIN32)
set(MSGPACK11_LIBRARIES "msgpack11.lib")
link_libraries(${MSGPACK11_LIBRARIES})
link_directories("${PROJECT_BINARY_DIR}/msgpack11/$<CONFIGURATION>;${PROJECT_BINARY_DIR}/../msgpack11/$<CONFIGURATION>")
else()
set(MSGPACK11_LIBRARIES "libmsgpack11.a")
link_directories("${PROJECT_BINARY_DIR}/../msgpack11")
endif()
include_directories(${MSGPACK11_INCLUDE_DIR})
# sick_scansegment_xd sources
file(GLOB SCANSEGMENT_XD_SOURCES driver/src/sick_scansegment_xd/*.cpp)
# msgpack11 sources and header required for SCANSEGMENT_XD support
include_directories(include/sick_scansegment_xd/msgpack11)
file(GLOB SCANSEGMENT_XD_SOURCES driver/src/sick_scansegment_xd/*.cpp driver/src/sick_scansegment_xd/msgpack11/*.cpp)
else()
message(STATUS "Building ${PROJECT_NAME} without SCANSEGMENT_XD support")
endif()
Expand Down
2 changes: 1 addition & 1 deletion INSTALL-ROS2.md
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ Run the following steps to build sick_scan_xd on Linux with ROS 2:

Note: LDMRS sensors are currently not supported on Raspberry. Build with cmake flag `-DLDMRS=0 -DRASPBERRY=1` on Raspberry:
```
colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DRASPBERRY=0" --event-handlers console_direct+
colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DRASPBERRY=0" --event-handlers console_direct+
```

Note: libsick_ldmrs is only required to support LDMRS sensors. If you do not need or want to support LDMRS, you can skip building libsick_ldmrs. To build sick_generic_caller without LDMRS support, switch off option `BUILD_WITH_LDMRS_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call colcon with option `-DLDMRS=0`:
Expand Down
5 changes: 3 additions & 2 deletions driver/src/ldmrs/sick_ldmrs_driver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -683,10 +683,11 @@ void ldmrsScanToPointCloud2(const datatypes::Scan* scan, sick_scan_xd::SickCloud
{
const ScanPoint& p = (*scan)[i];
float range = p.getDist();
if (range_filter.apply(range)) // otherwise point dropped by range filter
bool range_modified = false;
if (range_filter.apply(range, range_modified)) // otherwise point dropped by range filter
{
float azi = p.getHAngle(), ele = -p.getVAngle();
if (fabs(range - p.getDist()) > FLT_EPSILON) // range modified, transform polar to cartesian
if (range_modified) // range modified, transform polar to cartesian
{
data_p->x = range * cos(ele) * cos(azi);
data_p->y = range * cos(ele) * sin(azi);
Expand Down
10 changes: 9 additions & 1 deletion driver/src/sick_generic_caller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -138,6 +138,12 @@ int main(int argc, char** argv)
#if defined __ROS_VERSION && __ROS_VERSION == 2
// Pass command line arguments to rclcpp.
rclcpp::init(argc, argv);
bool ros_signal_handler = rclcpp::signal_handlers_installed();
ROS_INFO_STREAM("ROS2 signal handler are " << (ros_signal_handler? "" : " NOT") << "installed");
if (rclcpp::uninstall_signal_handlers())
ROS_INFO_STREAM("ROS2 signal handler uninstalled");
else
ROS_ERROR_STREAM("## ERROR: Failed to uninstall ROS2 signal handler");
rclcpp::NodeOptions node_options;
node_options.allow_undeclared_parameters(true);
//node_options.automatically_declare_initial_parameters(true);
Expand All @@ -148,7 +154,8 @@ int main(int argc, char** argv)
ros::NodeHandle nh("~");
rosNodePtr node = &nh;
#endif
signal(SIGINT, rosSignalHandler);
signal(SIGINT, rosSignalHandler); // SIGINT = 2, Ctrl-C or kill -2
signal(SIGTERM, rosSignalHandler); // SIGTERM = 15, default kill level

ROS_INFO_STREAM(versionInfo);
for (int i = 0; i < argc_tmp; i++)
Expand All @@ -175,6 +182,7 @@ int main(int argc, char** argv)
rosSpin(node);
}
stopScannerAndExit();
joinGenericLaser();
}
catch(const std::exception& e)
{
Expand Down
33 changes: 25 additions & 8 deletions driver/src/sick_generic_laser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@
#include <signal.h>

#define SICK_GENERIC_MAJOR_VER "3"
#define SICK_GENERIC_MINOR_VER "0"
#define SICK_GENERIC_MINOR_VER "1"
#define SICK_GENERIC_PATCH_LEVEL "0"

#define DELETE_PTR(p) if(p){delete(p);p=0;}
Expand Down Expand Up @@ -129,7 +129,10 @@ class GenericLaserCallable
}
void join(void)
{
generic_laser_thread->join();
if(generic_laser_thread && generic_laser_thread->joinable())
{
generic_laser_thread->join();
}
}
int argc;
char** argv;
Expand Down Expand Up @@ -181,13 +184,8 @@ bool stopScannerAndExit(bool force_immediate_shutdown)
success = s_scanner->stopScanData(force_immediate_shutdown);
}
runState = scanner_finalize;
if(s_generic_laser_thread)
{
s_generic_laser_thread->join();
delete s_generic_laser_thread;
s_generic_laser_thread = 0;
}
}
joinGenericLaser();
return success;
}

Expand All @@ -203,10 +201,14 @@ void rosSignalHandler(int signalRecv)
ROS_INFO_STREAM("You are leaving the following version of this node:\n");
ROS_INFO_STREAM(getVersionInfo() << "\n");
s_shutdownSignalReceived = true;
std::this_thread::sleep_for(std::chrono::milliseconds(100));
stopScannerAndExit(true);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
std::cout << "rosSignalHandler exit." << std::endl;
rosShutdown();
}


inline bool ends_with(std::string const &value, std::string const &ending)
{
if (ending.size() > value.size())
Expand Down Expand Up @@ -433,6 +435,7 @@ void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNo
{
#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0
exit_code = sick_scansegment_xd::run(nhPriv, scannerName);
std::cout << "sick_generic_laser: sick_scansegment_xd finished with " << (exit_code == sick_scan_xd::ExitSuccess ? "success" : "ERROR") << std::endl;
return;
#else
ROS_ERROR_STREAM("SCANSEGMENT_XD_SUPPORT deactivated, " << scannerName << " not supported. Please build sick_scan_xd with option SCANSEGMENT_XD_SUPPORT");
Expand Down Expand Up @@ -676,11 +679,25 @@ bool startGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr n
{
if (s_generic_laser_thread == 0)
{
runState = scanner_init;
s_generic_laser_thread = new GenericLaserCallable(argc, argv, nodeName, nhPriv, exit_code);
}
return (s_generic_laser_thread != 0);
}

/*!
\brief Waits until all GenericLaser jobs finished.
*/
void joinGenericLaser(void)
{
if (s_generic_laser_thread != 0)
{
s_generic_laser_thread->join();
delete s_generic_laser_thread;
s_generic_laser_thread = 0;
}
}

/*!
\brief Internal Startup routine.
\param argc: Number of Arguments
Expand Down
4 changes: 2 additions & 2 deletions driver/src/sick_generic_radar.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1619,14 +1619,14 @@ namespace sick_scan_xd
size_t numFilteredTargets = 0;
for (int i = 0; i < numTargets; i++)
{
bool tgt_valid = true;
bool tgt_valid = true, range_modified = false;
switch (iLoop)
{
case 0:
{
float angle = deg2rad * rawTargetList[i].Azimuth();
float range = rawTargetList[i].Dist();
tgt_valid = m_range_filter.apply(range);
tgt_valid = m_range_filter.apply(range, range_modified);
if (tgt_valid)
{
valSingle[0] = range * cos(angle);
Expand Down
98 changes: 64 additions & 34 deletions driver/src/sick_scan_common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -874,7 +874,7 @@ namespace sick_scan_xd
{
delete cloud_marker_;
delete diagnosticPub_;
printf("sick_scan_xd driver closed.\n");
printf("SickScanCommon closed.\n");
}


Expand Down Expand Up @@ -1494,7 +1494,7 @@ namespace sick_scan_xd
sopasCmdVec[CMD_START_MEASUREMENT] = "\x02sMN LMCstartmeas\x03";
sopasCmdVec[CMD_STOP_MEASUREMENT] = "\x02sMN LMCstopmeas\x03";
sopasCmdVec[CMD_APPLICATION_MODE_FIELD_ON] = "\x02sWN SetActiveApplications 1 FEVL 1\x03"; // <STX>sWN{SPC}SetActiveApplications{SPC}1{SPC}FEVL{SPC}1<ETX>
sopasCmdVec[CMD_APPLICATION_MODE_FIELD_OFF] = "\x02sWN SetActiveApplications 1 FEVL 0\x03"; // <STX>sWN{SPC}SetActiveApplications{SPC}1{SPC}FEVL{SPC}1<ETX>
sopasCmdVec[CMD_APPLICATION_MODE_FIELD_OFF] = "\x02sWN SetActiveApplications 1 FEVL 0\x03"; // <STX>sWN{SPC}SetActiveApplications{SPC}1{SPC}FEVL{SPC}0<ETX>
sopasCmdVec[CMD_APPLICATION_MODE_RANGING_ON] = "\x02sWN SetActiveApplications 1 RANG 1\x03";
sopasCmdVec[CMD_SET_TO_COLA_A_PROTOCOL] = "\x02sWN EIHstCola 0\x03";
sopasCmdVec[CMD_GET_PARTIAL_SCANDATA_CFG] = "\x02sRN LMDscandatacfg\x03";//<STX>sMN{SPC}mLMPsetscancfg{SPC } +5000{SPC}+1{SPC}+5000{SPC}-450000{SPC}+2250000<ETX>
Expand Down Expand Up @@ -1585,14 +1585,13 @@ namespace sick_scan_xd
sopasCmdMaskVec[CMD_APPLICATION_MODE] = "\x02sWN SetActiveApplications 1 %s %d\x03";
sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES] = "\x02sWN LMPoutputRange 1 %X %X %X\x03";
sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES_NAV3] = "\x02sWN LMPoutputRange 1 %X %X %X %X %X %X %X %X %X %X %X %X\x03";
//sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG]= "\x02sWN LMDscandatacfg %02d 00 %d 00 %d 0 %d 0 0 0 1 +1\x03"; //outputChannelFlagId,rssiFlag, rssiResolutionIs16Bit ,EncoderSetings
sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG] = "\x02sWN LMDscandatacfg %02d 00 %d %d 0 0 %02d 0 0 0 1 1\x03";//outputChannelFlagId,rssiFlag, rssiResolutionIs16Bit ,EncoderSetings
sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG] = "\x02sWN LMDscandatacfg %02d 00 %d %d 0 0 %02d 0 0 0 %d 1\x03"; // outputChannelFlagId, rssiFlag, rssiResolutionIs16Bit, EncoderSettings, timingflag
/*
configuration
* in ASCII
* sWN LMDscandatacfg %02d 00 %d %d 0 %02d 0 0 0 1 1
* | | | | | | | | | | +----------> Output rate -> All scans: 1--> every 1 scan
* | | | | | | | | +----------------> Time ->True (unused in Data Processing)
* | | | | | | | | +----------------> Time ->True (unused in Data Processing, TiM240:false)
* | | | | | | | +-------------------> Comment ->False
* | | | | | | +------------------------> Device Name ->False
* | | | | | +---------------------------> Position ->False
Expand Down Expand Up @@ -1782,9 +1781,9 @@ namespace sick_scan_xd
switch (numberOfLayers)
{
case 4:
sopasCmdChain.push_back(CMD_DEVICE_IDENT);
sopasCmdChain.push_back(CMD_APPLICATION_MODE_FIELD_OFF);
sopasCmdChain.push_back(CMD_APPLICATION_MODE_RANGING_ON);
sopasCmdChain.push_back(CMD_DEVICE_IDENT);
sopasCmdChain.push_back(CMD_SERIAL_NUMBER);

break;
Expand Down Expand Up @@ -2276,6 +2275,7 @@ namespace sick_scan_xd
{
return ExitFatal;
}
deviceIdentStr = deviceIdent;
// ROS_ERROR("BINARY REPLY REQUIRED");
}
else
Expand Down Expand Up @@ -2311,6 +2311,7 @@ namespace sick_scan_xd
{
return ExitFatal;
}
deviceIdentStr = fullIdentVersionInfo;

}
break;
Expand All @@ -2332,7 +2333,8 @@ namespace sick_scan_xd
{
return ExitFatal;
}
}
deviceIdentStr = sopasReplyStrVec[CMD_DEVICE_IDENT_LEGACY];
}
break;
/*
DEVICE_STATE
Expand Down Expand Up @@ -3241,32 +3243,46 @@ namespace sick_scan_xd
{
if (false==this->parser_->getCurrentParamPtr()->getUseScancfgList())
{
// Timing flag LMDscandatacfg (LMS-1XX, LMS-1XXX, LMS-4XXX, LMS-5XX, MRS-1XXX, MRS-6XXX, NAV-2XX, TIM-240, TIM-4XX, TIM-5XX, TIM-7XX, TIM-7XXS):
// -1: use default (i.e. off for TiM-240, otherwise on), 0: do not send time information, 1: send time information
int scandatacfg_timingflag = -1;
rosDeclareParam(nh, "scandatacfg_timingflag", scandatacfg_timingflag);
rosGetParam(nh, "scandatacfg_timingflag", scandatacfg_timingflag);
if (scandatacfg_timingflag < 0)
{
if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_TIM_240_NAME) == 0)
scandatacfg_timingflag = 0; // default for TiM-240: Timing flag LMDscandatacfg off
else
scandatacfg_timingflag = 1; // default: Timing flag LMDscandatacfg on
}

//normal scanconfig handling
char requestLMDscandatacfg[MAX_STR_LEN];
// Uses sprintf-Mask to set bitencoded echos and rssi enable flag
// sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG] = "\x02sWN LMDscandatacfg %02d 00 %d %d 00 %d 00 0 0 0 1 1\x03";
const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG].c_str();
sprintf(requestLMDscandatacfg, pcCmdMask, outputChannelFlagId, rssiFlag ? 1 : 0,
char requestLMDscandatacfg[MAX_STR_LEN];
// Uses sprintf-Mask to set bitencoded echos and rssi enable flag
// sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG] = "\x02sWN LMDscandatacfg %02d 00 %d %d 00 %d 00 0 0 0 1 %d\x03"; // outputChannelFlagId, rssiFlag, rssiResolutionIs16Bit, EncoderSettings, timingflag
const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG].c_str();
sprintf(requestLMDscandatacfg, pcCmdMask, outputChannelFlagId, rssiFlag ? 1 : 0,
rssiResolutionIs16Bit ? 1 : 0,
EncoderSettings != -1 ? EncoderSettings : 0);
if (useBinaryCmd)
{
std::vector<unsigned char> reqBinary;
this->convertAscii2BinaryCmd(requestLMDscandatacfg, &reqBinary);
// FOR MRS6124 this should be
// like this:
// 0000 02 02 02 02 00 00 00 20 73 57 4e 20 4c 4d 44 73 .......sWN LMDs
// 0010 63 61 6e 64 61 74 61 63 66 67 20 1f 00 01 01 00 candatacfg .....
// 0020 00 00 00 00 00 00 00 01 5c
result = sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_SET_PARTIAL_SCANDATA_CFG]);
RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, sopasReplyBinVec[CMD_SET_PARTIAL_SCANDATA_CFG]); // No response, non-recoverable connection error (return error and do not try other commands)
}
else
{
std::vector<unsigned char> lmdScanDataCfgReply;
result = sendSopasAndCheckAnswer(requestLMDscandatacfg, &lmdScanDataCfgReply);
RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, lmdScanDataCfgReply); // No response, non-recoverable connection error (return error and do not try other commands)
}
EncoderSettings != -1 ? EncoderSettings : 0,
scandatacfg_timingflag);
if (useBinaryCmd)
{
std::vector<unsigned char> reqBinary;
this->convertAscii2BinaryCmd(requestLMDscandatacfg, &reqBinary);
// FOR MRS6124 this should be
// like this:
// 0000 02 02 02 02 00 00 00 20 73 57 4e 20 4c 4d 44 73 .......sWN LMDs
// 0010 63 61 6e 64 61 74 61 63 66 67 20 1f 00 01 01 00 candatacfg .....
// 0020 00 00 00 00 00 00 00 01 5c
result = sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_SET_PARTIAL_SCANDATA_CFG]);
RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, sopasReplyBinVec[CMD_SET_PARTIAL_SCANDATA_CFG]); // No response, non-recoverable connection error (return error and do not try other commands)
}
else
{
std::vector<unsigned char> lmdScanDataCfgReply;
result = sendSopasAndCheckAnswer(requestLMDscandatacfg, &lmdScanDataCfgReply);
RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, lmdScanDataCfgReply); // No response, non-recoverable connection error (return error and do not try other commands)
}
}
else
{
Expand Down Expand Up @@ -3666,7 +3682,7 @@ namespace sick_scan_xd
startProtocolSequence.push_back(CMD_START_SCANDATA);
}

if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() == 4) // MRS1104 - start IMU-Transfer
if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() == 4) // MRS1104: start IMU-Transfer
{
bool imu_enable = false;
rosDeclareParam(nh, "imu_enable", imu_enable);
Expand All @@ -3685,7 +3701,10 @@ namespace sick_scan_xd
"IMU USAGE NOT POSSIBLE IN ASCII COMMUNICATION MODE.\nTo use the IMU the communication with the scanner must be set to binary mode.\n This can be done by inserting the line:\n<param name=\"use_binary_protocol\" type=\"bool\" value=\"True\" />\n into the launchfile.\n See also https://github.com/SICKAG/sick_scan_xd/blob/master/doc/IMU.md");
exit(0);
}

}
else
{
ROS_INFO("IMU data transfer not enabled");
}
}
}
Expand Down Expand Up @@ -4975,7 +4994,8 @@ namespace sick_scan_xd

// Apply range filter
float range_meter = rangeTmpPtr[iEcho * rangeNum + rangeIdxScan];
if (range_filter.apply(range_meter)) // otherwise point dropped by range filter
bool range_modified = false;
if (range_filter.apply(range_meter, range_modified)) // otherwise point dropped by range filter
{
// ROS_DEBUG_STREAM("alpha:" << alpha << " elevPreCalc:" << std::to_string(elevationPreCalculated) << " layer:" << layer << " elevDeg:" << elevationAngleDegree
// << " numOfLayers:" << numOfLayers << " elevAngleX200:" << elevAngleX200);
Expand Down Expand Up @@ -5440,6 +5460,16 @@ namespace sick_scan_xd
buffer[2 + ii] = szApplStr[ii]; // idx: 1,2,3,4
}
buffer[6] = dummy1 ? 0x01 : 0x00;
if (buffer[6] == 0x00 && parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_MRS_1XXX_NAME) == 0) // activate FEVL in case of MRS1xxx with firmware version > 1
{
size_t device_idx = deviceIdentStr.find("MRS1xxx"); // Get MRS1xxx version from device ident string
size_t version_idx = ((device_idx != std::string::npos) ? deviceIdentStr.find("V", device_idx) : std::string::npos);
char version_id = ((version_idx != std::string::npos) ? deviceIdentStr[version_idx + 1] : '0');
if (version_id > '1')
{
buffer[6] = 0x01; // MRS1xxx with firmware version > 1 supports RANG+FEVL -> overwrite with "<STX>sWN{SPC}SetActiveApplications{SPC}1{SPC}FEVL{SPC}1<ETX>"
}
}
bufferLen = 7;
}

Expand Down
Loading

0 comments on commit e5f7206

Please sign in to comment.