From d1e6d93b207da8ebe5f66eba247ae3be9cb1955b Mon Sep 17 00:00:00 2001 From: rostest Date: Mon, 15 Feb 2021 09:44:37 +0100 Subject: [PATCH] Release 1.9.0 field monitoring for TiM7xx and TiM7xxS --- CMakeLists.txt | 4 +- README.md | 2 +- doc/tim7xxs_extensions.md | 6 +- driver/src/sick_generic_parser.cpp | 42 +++++++------ driver/src/sick_scan_common.cpp | 11 ++-- include/sick_scan/sick_generic_parser.h | 15 ++++- launch/sick_tim_7xx.launch | 4 ++ launch/sick_tim_7xxS.launch | 2 +- test/emulator/config/rviz_emulator_cfg.rviz | 13 +++- test/scripts/run_simu_tim781s.bash | 63 ------------------- test/scripts/run_simu_tim7xx_tim7xxS.bash | 68 +++++++++++++++++++++ test/scripts/vs_code.bash | 2 +- 12 files changed, 129 insertions(+), 103 deletions(-) mode change 100644 => 100755 README.md delete mode 100755 test/scripts/run_simu_tim781s.bash create mode 100755 test/scripts/run_simu_tim7xx_tim7xxS.bash diff --git a/CMakeLists.txt b/CMakeLists.txt index fb1be61..91bde50 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,8 +6,8 @@ cmake_minimum_required(VERSION 2.8.3) # build options: set OFF for relese version, ON for development and test -option(ENABLE_EMULATOR "Build emulator for offline and unittests" OFF) # OFF (release) or ON (development) -option(BUILD_DEBUG_TARGET "Build debug target" OFF) # OFF (release) or ON (development) +option(ENABLE_EMULATOR "Build emulator for offline and unittests" ON) # OFF (release) or ON (development) +option(BUILD_DEBUG_TARGET "Build debug target" ON) # OFF (release) or ON (development) # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wpedantic") diff --git a/README.md b/README.md old mode 100644 new mode 100755 index e1bf31b..58fde00 --- a/README.md +++ b/README.md @@ -213,7 +213,7 @@ The use of the parameters can be looked up in the launch files. This is also rec - Angle compensation: For highest angle accuracy the NAV-Lidar series supports an [angle compensation mechanism](./doc/angular_compensation.md). -- The **TiM7xxS** family has [extended settings for field monitoring](./doc/tim7xxs_extensions.md). +- The **TiM7xx** and **TiM7xxS** families have [extended settings for field monitoring](./doc/tim7xxs_extensions.md). ## Sopas Mode This driver supports both COLA-B (binary) and COLA-A (ASCII) communication with the laser scanner. Binary mode is activated by default. Since this mode generates less network traffic. diff --git a/doc/tim7xxs_extensions.md b/doc/tim7xxs_extensions.md index 438210e..e9761df 100755 --- a/doc/tim7xxs_extensions.md +++ b/doc/tim7xxs_extensions.md @@ -1,12 +1,12 @@ # Extensions for TiM7xxS -The TiM7xxS family has the following extended settings for field monitoring: +The TiM7xx and TiM7xxS families have the following extended settings for field monitoring: ## Field monitoring messages -TiM7xxS scanner support field monitoring. Fields can be configured by Sopas ET. Once they are configured, sick_scan publishes ros messages containing the monitoring information from the lidar. +TiM7xx and TiM7xxS scanner support field monitoring. Fields can be configured by Sopas ET. Once they are configured, sick_scan publishes ros messages containing the monitoring information from the lidar. -By default, field monitoring is enabled in the launch file [sick_tim_7xxS.launch](../launch/sick_tim_7xxS.launch) by following settings: +By default, field monitoring is enabled in the launch file [sick_tim_7xx.launch](../launch/sick_tim_7xx.launch) resp. [sick_tim_7xxS.launch](../launch/sick_tim_7xxS.launch) by following settings: ``` diff --git a/driver/src/sick_generic_parser.cpp b/driver/src/sick_generic_parser.cpp index c6cdcc0..6542d81 100644 --- a/driver/src/sick_generic_parser.cpp +++ b/driver/src/sick_generic_parser.cpp @@ -332,13 +332,16 @@ namespace sick_scan return (useSafetyPasWD); } - bool ScannerBasicParam::getUseSafetyFields(){ - return(useSafetyFields); + EVAL_FIELD_SUPPORT ScannerBasicParam::getUseEvalFields() + { + return this->useEvalFields; } - void ScannerBasicParam::setUseSafetyFields(bool _useSafetyFields){ - this->useSafetyFields=_useSafetyFields; + void ScannerBasicParam::setUseEvalFields(EVAL_FIELD_SUPPORT _useEvalFields) + { + this->useEvalFields = _useEvalFields; } + /*! \brief Construction of parameter object @@ -346,7 +349,7 @@ namespace sick_scan 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) + CartographerCompatibility(false), scanMirroredAndShifted(false), useEvalFields(EVAL_FIELD_UNSUPPORTED) { this->elevationDegreeResolution = 0.0; this->setUseBinaryProtocol(false); @@ -420,7 +423,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0) // LMS1000 - 4 layer, 1101 shots per scan @@ -436,7 +439,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_240_NAME) == 0) // TIM_5xx - 1 Layer, max. 811 shots per scan @@ -451,7 +454,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_5XX_NAME) == @@ -467,8 +470,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); - + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_4XXX_NAME) == 0) // LMS_4xxx - 1 Layer, 600 Hz { @@ -482,7 +484,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XX_NAME) == 0) // TIM_7xx - 1 Layer Scanner { @@ -496,7 +498,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(USE_EVAL_FIELD_TIM7XX_LOGIC); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XXS_NAME) == 0) // TIM_7xxS - 1 layer Safety Scanner { @@ -510,7 +512,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(true); // Safety scanner basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(true); + basicParams[i].setUseEvalFields(USE_EVAL_FIELD_TIM7XX_LOGIC); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0) // LMS_5xx - 1 Layer { @@ -524,7 +526,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XX_NAME) == 0) // LMS_1xx - 1 Layer { @@ -538,7 +540,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_MRS_6XXX_NAME) == 0) // { @@ -553,7 +555,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_3XX_NAME) == 0) // Radar @@ -569,7 +571,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) // Nav 3xx { @@ -581,7 +583,7 @@ namespace sick_scan basicParams[i].setUseBinaryProtocol(true); basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setScanMirroredAndShifted(true); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_2XX_NAME) == 0) // NAV_2xx - 1 Layer { @@ -595,7 +597,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_4XX_NAME) == 0) // TiM433 and TiM443 { @@ -609,7 +611,7 @@ namespace sick_scan basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseSafetyFields(false); + basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); } } diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp index 20bd085..7f4c002 100644 --- a/driver/src/sick_scan_common.cpp +++ b/driver/src/sick_scan_common.cpp @@ -371,7 +371,7 @@ namespace sick_scan publish_lferec_ = false; publish_lidoutputstate_ = false; const std::string scannername = parser_->getCurrentParamPtr()->getScannerName(); - if (scannername.compare(SICK_SCANNER_TIM_7XXS_NAME) == 0) + if (parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) { lferec_pub_ = nh_.advertise(scannername + "/lferec", 100); lidoutputstate_pub_ = nh_.advertise(scannername + "/lidoutputstate", 100); @@ -435,7 +435,7 @@ namespace sick_scan printf("\nSOPAS - Stopped streaming scan data.\n"); } - if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_TIM_7XXS_NAME) == 0) + if (parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) { if(sendSOPASCommand("\x02sEN LFErec 0\x03", NULL) != 0 // TiM781S: deactivate LFErec messages, send "sEN LFErec 0" || sendSOPASCommand("\x02sEN LIDoutputstate 0\x03", NULL) != 0 // TiM781S: deactivate LIDoutputstate messages, send "sEN LIDoutputstate 0" @@ -1170,13 +1170,11 @@ namespace sick_scan bool rssiFlag = false; bool rssiResolutionIs16Bit = true; //True=16 bit Flase=8bit - // bool useSafetyfields=false; int activeEchos = 0; ros::NodeHandle pn("~"); pn.getParam("intensity", rssiFlag); pn.getParam("intensity_resolution_16bit", rssiResolutionIs16Bit); - // 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; @@ -1215,7 +1213,6 @@ namespace sick_scan } this->parser_->getCurrentParamPtr()->setIntensityResolutionIs16Bit(rssiResolutionIs16Bit); - // this->parser_->getCurrentParamPtr()->setUseSafetyFields(useSafetyfields); // parse active_echos entry and set flag array pn.getParam("active_echos", activeEchos); @@ -2106,7 +2103,7 @@ namespace sick_scan } //SAFTY FIELD PARSING - if (this->parser_->getCurrentParamPtr()->getUseSafetyFields()) + if (this->parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) { ROS_INFO("Reading safety fields"); SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); @@ -2443,7 +2440,7 @@ namespace sick_scan startProtocolSequence.push_back(CMD_RUN); // leave user level startProtocolSequence.push_back(CMD_START_SCANDATA); - if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_TIM_7XXS_NAME) == 0) + if (parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) { // Activate LFErec, LIDoutputstate and LIDinputstate messages diff --git a/include/sick_scan/sick_generic_parser.h b/include/sick_scan/sick_generic_parser.h index 9b8ed42..e01a8b8 100644 --- a/include/sick_scan/sick_generic_parser.h +++ b/include/sick_scan/sick_generic_parser.h @@ -58,6 +58,15 @@ // namespace sensor_msgs namespace sick_scan { + enum EVAL_FIELD_SUPPORT // type of eval field support: + { + EVAL_FIELD_UNSUPPORTED = 0, // Lidar does not support eval field (default) + USE_EVAL_FIELD_TIM7XX_LOGIC, // eval fields supported by TiM7xx and TiM7xxS + USE_EVAL_FIELD_LMS5XX_LOGIC, // eval fields supported by LMS5XX + USE_EVAL_FIELD_LMS5XX_UNSUPPORTED, // eval fields not supported by LMS5XX + USE_EVAL_FIELD_NUM // max number of eval field support types + }; + class ScannerBasicParam { public: @@ -115,9 +124,9 @@ namespace sick_scan int8_t getEncoderMode(); - bool getUseSafetyFields(); + EVAL_FIELD_SUPPORT getUseEvalFields(); - void setUseSafetyFields(bool _useSafetyFields); + void setUseEvalFields(EVAL_FIELD_SUPPORT _useEvalFields); private: std::string scannerName; @@ -134,7 +143,7 @@ namespace sick_scan int8_t encoderMode; bool CartographerCompatibility; bool scanMirroredAndShifted; - bool useSafetyFields; + EVAL_FIELD_SUPPORT useEvalFields; }; diff --git a/launch/sick_tim_7xx.launch b/launch/sick_tim_7xx.launch index 9e9471a..6327fc3 100644 --- a/launch/sick_tim_7xx.launch +++ b/launch/sick_tim_7xx.launch @@ -36,6 +36,10 @@ + + + + diff --git a/launch/sick_tim_7xxS.launch b/launch/sick_tim_7xxS.launch index 809d404..af20c21 100644 --- a/launch/sick_tim_7xxS.launch +++ b/launch/sick_tim_7xxS.launch @@ -36,7 +36,7 @@ - + diff --git a/test/emulator/config/rviz_emulator_cfg.rviz b/test/emulator/config/rviz_emulator_cfg.rviz index a579708..7303d46 100755 --- a/test/emulator/config/rviz_emulator_cfg.rviz +++ b/test/emulator/config/rviz_emulator_cfg.rviz @@ -8,6 +8,7 @@ Panels: - /PointCloud21 - /Axes1 - /MarkerArray1 + - /MarkerArray2 Splitter Ratio: 0.5 Tree Height: 799 - Class: rviz/Selection @@ -70,7 +71,7 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 39172 + Max Intensity: 38513 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 @@ -94,12 +95,20 @@ Visualization Manager: Value: true - Class: rviz/MarkerArray Enabled: true - Marker Topic: /sick_tim_7xxS/marker + Marker Topic: /sick_tim_7xx/marker Name: MarkerArray Namespaces: sick_scan: true Queue Size: 100 Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sick_tim_7xxS/marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/test/scripts/run_simu_tim781s.bash b/test/scripts/run_simu_tim781s.bash deleted file mode 100755 index bcec043..0000000 --- a/test/scripts/run_simu_tim781s.bash +++ /dev/null @@ -1,63 +0,0 @@ -#!/bin/bash -printf "\033c" -echo -e "run_simu_tim781.bash: starting TiM781S emulation\n" -pushd ../../../.. -source /opt/ros/melodic/setup.bash -source ./install/setup.bash - -# Start roscore if not yet running -roscore_running=`(ps -elf | grep roscore | grep -v grep | wc -l)` -if [ $roscore_running -lt 1 ] ; then - roscore & - sleep 3 -fi - -for emulator_launch_cfg in emulator_01_default.launch emulator_03_2nd_fieldset.launch emulator_02_toggle_fieldsets.launch ; do # emulator_04_fieldset_test.launch - echo -e "Starting TiM781S emulation $emulator_launch_cfg, shutdown ros nodes\n" - - # Start sick_scan emulator - roslaunch sick_scan $emulator_launch_cfg & - sleep 1 - - # Start rviz - # Note: Due to a bug in opengl 3 in combination with rviz and VMware, opengl 2 should be used by rviz option --opengl 210 - # See https://github.com/ros-visualization/rviz/issues/1444 and https://github.com/ros-visualization/rviz/issues/1508 for further details - rosrun rviz rviz -d ./src/sick_scan/test/emulator/config/rviz_emulator_cfg.rviz --opengl 210 & - sleep 1 - - # Start sick_scan driver for TiM781S - # roslaunch sick_scan sick_tim_7xx.launch hostname:=127.0.0.1 & - roslaunch sick_scan sick_tim_7xxS.launch hostname:=127.0.0.1 & - sleep 1 - - # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDoutputstate 1'}" - # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LIDoutputstate'}" - # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LFErec 1'}" - # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LFErec'}" - # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN field000 1'}" - # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN field000'}" - # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDinputstate 1'}" - # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LIDinputstate'}" # response: "sRA LIDinputstate \\x00\\x00\\x00\\x00\\x00\\x00\\x01\\x00\\x00\\x00\\x00\\x00" - # rostopic echo "/sick_tim_7xxS/lferec" & - # rostopic echo "/sick_tim_7xxS/lidoutputstate" & - - # Wait for 'q' or 'Q' to exit or until rviz is closed - while true ; do - echo -e "TiM781S emulation running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key - if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi - rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)` - if [ $rviz_running -lt 1 ] ; then break ; fi - done - - # Shutdown - echo -e "Finishing TiM781S emulation $emulator_launch_cfg, shutdown ros nodes\n" - rosnode kill -a ; sleep 1 - killall sick_generic_caller ; sleep 1 - killall sick_scan_emulator ; sleep 1 - -done -killall rosmaster ; sleep 1 -echo -e "run_simu_tim781.bash: TiM781S emulation finished.\n\n" - -popd - diff --git a/test/scripts/run_simu_tim7xx_tim7xxS.bash b/test/scripts/run_simu_tim7xx_tim7xxS.bash new file mode 100755 index 0000000..d7bcd99 --- /dev/null +++ b/test/scripts/run_simu_tim7xx_tim7xxS.bash @@ -0,0 +1,68 @@ +#!/bin/bash +printf "\033c" +pushd ../../../.. +source /opt/ros/melodic/setup.bash +source ./install/setup.bash + +for sick_scan_launch_file in sick_tim_7xx.launch sick_tim_7xxS.launch ; do + echo -e "run_simu_tim7xx_tim7xxS.bash: starting TiM7xx/TiM7xxS emulation with $sick_scan_launch_file\n" + + # Start roscore if not yet running + roscore_running=`(ps -elf | grep roscore | grep -v grep | wc -l)` + if [ $roscore_running -lt 1 ] ; then + roscore & + sleep 3 + fi + + for emulator_launch_cfg in emulator_01_default.launch emulator_03_2nd_fieldset.launch emulator_02_toggle_fieldsets.launch ; do # emulator_04_fieldset_test.launch + echo -e "Starting TiM7xx/TiM7xxS emulation $emulator_launch_cfg\n" + + # Start sick_scan emulator + roslaunch sick_scan $emulator_launch_cfg & + sleep 1 + + # Start rviz + # Note: Due to a bug in opengl 3 in combination with rviz and VMware, opengl 2 should be used by rviz option --opengl 210 + # See https://github.com/ros-visualization/rviz/issues/1444 and https://github.com/ros-visualization/rviz/issues/1508 for further details + rosrun rviz rviz -d ./src/sick_scan/test/emulator/config/rviz_emulator_cfg.rviz --opengl 210 & + sleep 1 + + # Start sick_scan driver for TiM7xx/TiM7xxS + echo -e "Launching sick_scan $sick_scan_launch_file\n" + roslaunch sick_scan $sick_scan_launch_file hostname:=127.0.0.1 & + sleep 1 + + # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDoutputstate 1'}" + # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LIDoutputstate'}" + # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LFErec 1'}" + # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LFErec'}" + # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN field000 1'}" + # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN field000'}" + # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDinputstate 1'}" + # rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LIDinputstate'}" # response: "sRA LIDinputstate \\x00\\x00\\x00\\x00\\x00\\x00\\x01\\x00\\x00\\x00\\x00\\x00" + # rostopic echo "/sick_tim_7xxS/lferec" & + # rostopic echo "/sick_tim_7xxS/lidoutputstate" & + + # Wait for 'q' or 'Q' to exit or until rviz is closed + while true ; do + echo -e "TiM7xx/TiM7xxS emulation running ($sick_scan_launch_file, $emulator_launch_cfg). Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key + if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi + rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)` + if [ $rviz_running -lt 1 ] ; then break ; fi + done + + # Shutdown + echo -e "Finishing TiM7xx/TiM7xxS emulation $emulator_launch_cfg, shutdown ros nodes\n" + rosnode kill -a ; sleep 1 + killall sick_generic_caller ; sleep 1 + killall sick_scan_emulator ; sleep 1 + + done + killall rosmaster ; sleep 1 + echo -e "run_simu_tim7xx_tim7xxS.bash: TiM7xx/TiM7xxS emulation finished.\n\n" + +done +echo -e "run_simu_tim7xx_tim7xxS.bash: TiM7xx/TiM7xxS emulation finished.\n\n" + +popd + diff --git a/test/scripts/vs_code.bash b/test/scripts/vs_code.bash index 9e38552..2f61df5 100755 --- a/test/scripts/vs_code.bash +++ b/test/scripts/vs_code.bash @@ -1,6 +1,6 @@ #!/bin/bash -gedit ./make.bash ./run_simu_tim781s.bash & +gedit ./make.bash ./run_simu_tim7xx_tim7xxS.bash & pushd ../../../.. source /opt/ros/melodic/setup.bash