diff --git a/CHANGELOG.md b/CHANGELOG.md index 9d91e90e..a498fdd1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,9 @@ features that will be removed in future versions **Removed** for deprecated feat ## Released ## +### v2.5.1 - + - **Fixed** Error after SOPAS command SetAccessMode #27 + ### v2.5.0 - - **Fixed** Issue #24 (stop scanner at exit) - **Added** new ros service SickScanExit to stop scanner and exit diff --git a/README.md b/README.md index ad3bf400..e667325c 100644 --- a/README.md +++ b/README.md @@ -576,6 +576,18 @@ Note: * The COLA commands are sensor specific. See the user manual and telegram listing for further details. * ROS services require installation of ROS-1 or ROS-2, i.e. services for Cola commands are currently not supported on native Linux or native Windows. * ROS services are currently not available for the LDMRS. +* Some SOPAS commands like `sMN SetAccessMode 3 F4724744` stops the current measurement. In this case, the driver restarts after a timeout (5 seconds by default). To process those SOPAS commands without restart, you can + * send `sMN LMCstartmeas` and `sMN Run` to switch again into measurement mode within the timeout, or + * increase the driver timeout `read_timeout_millisec_default` in the launch-file. + +Example sequence with stop and start measurement to set a particle filter (TiM-7xxx on ROS-1): +``` +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LFPparticle'}" # response: "sRA LFPparticle \\x00\\x01\\xf4" +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sWN LFPparticle 0101F4'}" # response: "sWA LFPparticle" +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN LMCstartmeas'}" +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN Run'}" +``` ### Driver states, timeouts diff --git a/driver/src/sick_generic_caller.cpp b/driver/src/sick_generic_caller.cpp index a9a66f83..3fa43576 100644 --- a/driver/src/sick_generic_caller.cpp +++ b/driver/src/sick_generic_caller.cpp @@ -81,7 +81,7 @@ #define SICK_GENERIC_MAJOR_VER "2" #define SICK_GENERIC_MINOR_VER "5" -#define SICK_GENERIC_PATCH_LEVEL "0" +#define SICK_GENERIC_PATCH_LEVEL "1" #include // for std::min @@ -175,15 +175,24 @@ int main(int argc, char **argv) int result = 0; try { - result = mainGenericLaser(argc_tmp, argv_tmp, scannerName, node); + // result = mainGenericLaser(argc_tmp, argv_tmp, scannerName, node); + if (!startGenericLaser(argc_tmp, argv_tmp, scannerName, node, &result)) + { + ROS_ERROR_STREAM("## ERROR in sick_generic_caller::main(): startGenericLaser() failed, could not start generic laser event loop"); + } + else + { + rosSpin(node); + } + stopScannerAndExit(); } catch(const std::exception& e) { - ROS_ERROR_STREAM("## ERROR in mainGenericLaser: exception " << e.what()); + ROS_ERROR_STREAM("## ERROR in ick_generic_caller::main(): exception " << e.what()); } catch(...) { - ROS_ERROR_STREAM("## ERROR in mainGenericLaser: unknown exception "); + ROS_ERROR_STREAM("## ERROR in ick_generic_caller::main(): unknown exception "); } return result; diff --git a/driver/src/sick_generic_laser.cpp b/driver/src/sick_generic_laser.cpp index 043d05b2..586c5afb 100644 --- a/driver/src/sick_generic_laser.cpp +++ b/driver/src/sick_generic_laser.cpp @@ -105,6 +105,34 @@ std::string getVersionInfo() return (versionInfo); } +void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv, bool do_ros_spin, int & exit_code); + +class GenericLaserCallable +{ +public: + GenericLaserCallable(int _argc, char** _argv, std::string _nodeName, rosNodePtr _nhPriv, int* _exit_code) + : argc(_argc), argv(_argv), nodeName(_nodeName), nhPriv(_nhPriv), exit_code(_exit_code) + { + generic_laser_thread = new std::thread(&GenericLaserCallable::mainGenericLaserCb, this); + } + void mainGenericLaserCb(void) + { + mainGenericLaserInternal(argc, argv, nodeName, nhPriv, false, *exit_code); + } + void join(void) + { + generic_laser_thread->join(); + } + int argc; + char** argv; + std::string nodeName; + rosNodePtr nhPriv; + int* exit_code; + std::thread* generic_laser_thread; +}; + +static GenericLaserCallable* s_generic_laser_thread = 0; + NodeRunState runState = scanner_init; /*! @@ -145,6 +173,12 @@ 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; + } } return success; } @@ -244,14 +278,16 @@ bool parseLaunchfileSetParameter(rosNodePtr nhPriv, int argc, char **argv) \param argc: Number of Arguments \param argv: Argument variable \param nodeName name of the ROS-node -\return exit-code +\param nhPriv ros node handle +\param exit_code exit-code \sa main */ -int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv) +void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv, bool do_ros_spin, int & exit_code) { std::string tag; std::string val; + exit_code = sick_scan::ExitSuccess; bool doInternalDebug = false; bool emulSensor = false; for (int i = 0; i < argc; i++) @@ -284,6 +320,7 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP if(!parseLaunchfileSetParameter(nhPriv, argc, argv)) { ROS_ERROR_STREAM("## ERROR sick_generic_laser: parseLaunchfileSetParameter() failed, aborting\n"); + exit_code = sick_scan::ExitError; exit(-1); } #endif @@ -359,18 +396,21 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP #if defined LDMRS_SUPPORT && LDMRS_SUPPORT > 0 ROS_INFO("Initializing LDMRS..."); sick_scan::SickLdmrsNode ldmrs; - int result = ldmrs.init(nhPriv, hostname, frame_id); - if(result != sick_scan::ExitSuccess) + exit_code = ldmrs.init(nhPriv, hostname, frame_id); + if(exit_code != sick_scan::ExitSuccess) { ROS_ERROR("LDMRS initialization failed."); - return sick_scan::ExitError; + exit_code = sick_scan::ExitError; + return; } ROS_INFO("LDMRS initialized."); rosSpin(nhPriv); - return sick_scan::ExitSuccess; + exit_code = sick_scan::ExitSuccess; + return; #else ROS_ERROR("LDMRS not supported. Please build sick_scan with option LDMRS_SUPPORT"); - return sick_scan::ExitError; + exit_code = sick_scan::ExitError; + return; #endif } @@ -469,7 +509,7 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP bool start_services = true; sick_scan::SickScanServices* services = 0; - int result = sick_scan::ExitError; + exit_code = sick_scan::ExitError; //sick_scan::SickScanConfig cfg; //std::chrono::system_clock::time_point timestamp_rosOk = std::chrono::system_clock::now(); @@ -501,11 +541,11 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP { s_scanner->setEmulSensor(true); } - result = s_scanner->init(nhPriv); - if (result == sick_scan::ExitError || result == sick_scan::ExitFatal) + exit_code = s_scanner->init(nhPriv); + if (exit_code == sick_scan::ExitError || exit_code == sick_scan::ExitFatal) { ROS_ERROR("## ERROR in mainGenericLaser: init failed, retrying..."); // ROS_ERROR("init failed, shutting down"); - continue; // return result; + continue; } // Start ROS services @@ -520,7 +560,7 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP isInitialized = true; // signal(SIGINT, SIG_DFL); // change back to standard signal handler after initialising - if (result == sick_scan::ExitSuccess) // OK -> loop again + if (exit_code == sick_scan::ExitSuccess) // OK -> loop again { if (changeIP) { @@ -535,15 +575,18 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP break; case scanner_run: - if (result == sick_scan::ExitSuccess) // OK -> loop again + if (exit_code == sick_scan::ExitSuccess) // OK -> loop again { - rosSpinOnce(nhPriv); - result = s_scanner->loopOnce(nhPriv); + if(do_ros_spin) + { + rosSpinOnce(nhPriv); + } + exit_code = s_scanner->loopOnce(nhPriv); if(scan_msg_monitor && message_monitoring_enabled) // Monitor scanner messages { - result = scan_msg_monitor->checkStateReinitOnError(nhPriv, runState, s_scanner, parser, services); - if(result != sick_scan::ExitSuccess) // scanner re-init failed after read timeout or tcp error + exit_code = scan_msg_monitor->checkStateReinitOnError(nhPriv, runState, s_scanner, parser, services); + if(exit_code != sick_scan::ExitSuccess) // scanner re-init failed after read timeout or tcp error { ROS_ERROR("## ERROR in sick_generic_laser main loop: read timeout, scanner re-init failed"); } @@ -569,6 +612,39 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP DELETE_PTR(services); DELETE_PTR(s_scanner); // close connnect DELETE_PTR(parser); // close parser - return result; + return; +} +/*! +\brief Runs mainGenericLaser non-blocking in a new thread +\param argc: Number of Arguments +\param argv: Argument variable +\param nodeName name of the ROS-node +\param nhPriv ros node handle +\param exit_code exit-code +\return true if mainGenericLaser is running, false otherwise +\sa mainGenericLaser +*/ +bool startGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv, int* exit_code) +{ + if (s_generic_laser_thread == 0) + { + s_generic_laser_thread = new GenericLaserCallable(argc, argv, nodeName, nhPriv, exit_code); + } + return (s_generic_laser_thread != 0); +} + +/*! +\brief Internal Startup routine. +\param argc: Number of Arguments +\param argv: Argument variable +\param nodeName name of the ROS-node +\return exit-code +\sa main +*/ +int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv) +{ + int result; + mainGenericLaserInternal(argc, argv, nodeName, nhPriv, true, result); + return result; } diff --git a/include/sick_scan/sick_generic_laser.h b/include/sick_scan/sick_generic_laser.h index 40262869..9d88ab59 100644 --- a/include/sick_scan/sick_generic_laser.h +++ b/include/sick_scan/sick_generic_laser.h @@ -14,6 +14,8 @@ enum NodeRunState }; +bool startGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv, int* exit_code); + int mainGenericLaser(int argc, char **argv, std::string scannerName, rosNodePtr nh); void rosSignalHandler(int signalRecv); diff --git a/launch/sick_lrs_4xxx.launch b/launch/sick_lrs_4xxx.launch index 6f59c848..e0ee29ef 100644 --- a/launch/sick_lrs_4xxx.launch +++ b/launch/sick_lrs_4xxx.launch @@ -8,8 +8,8 @@ - - + +