├── .local_test.sh ├── .travis.sh ├── .travis.yml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── launch ├── dbg_safety_control.launch └── safety_control.launch ├── package.xml └── src └── safety_control_node.cpp /.local_test.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | docker run --rm -i -v `pwd`:`pwd` -e "CI_SOURCE_PATH=`pwd`" -e "ROS_DISTRO=kinetic" -t ubuntu:xenial sh -c "cd `pwd`; ./.travis.sh" 3 | -------------------------------------------------------------------------------- /.travis.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | export DEBIAN_FRONTEND noninteractive 5 | export TERM xterm 6 | 7 | apt-get update && apt-get install -y -q wget sudo lsb-release # for docker 8 | 9 | #before_install: 10 | sh -c "echo \"deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main\" > /etc/apt/sources.list.d/ros-latest.list" 11 | wget http://packages.ros.org/ros.key -O - | apt-key add - 12 | apt-get update && apt-get install -y -q python-catkin-pkg python-rosdep python-wstool ros-kinetic-catkin build-essential 13 | source /opt/ros/$ROS_DISTRO/setup.bash 14 | rosdep init 15 | rosdep update 16 | 17 | #install: 18 | mkdir -p ~/catkin_ws/src 19 | cd ~/catkin_ws/src 20 | catkin_init_workspace 21 | cd ~/catkin_ws 22 | catkin_make 23 | source devel/setup.bash 24 | cd ~/catkin_ws/src 25 | ln -s $CI_SOURCE_PATH . 26 | 27 | #before_script: 28 | cd ~/catkin_ws 29 | rosdep install -q -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO 30 | 31 | #script: 32 | source /opt/ros/$ROS_DISTRO/setup.bash 33 | cd ~/catkin_ws 34 | catkin_make 35 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | 5 | env: 6 | - ROS_DISTRO=kinetic 7 | 8 | before_install: 9 | - export CI_SOURCE_PATH=$(pwd) 10 | - export REPOSITORY_NAME=${PWD##*/} 11 | 12 | script: 13 | - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" 14 | - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -t ubuntu:xenial sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" 15 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(safety_control) 3 | 4 | set(CMAKE_BUILD_TYPE Debug) 5 | 6 | ## Find catkin dependencies 7 | find_package(catkin REQUIRED COMPONENTS 8 | pcl_conversions 9 | pcl_ros 10 | roscpp 11 | rospy 12 | std_msgs 13 | geometry_msgs 14 | trajectory_msgs 15 | tf 16 | nav_msgs 17 | sensor_msgs 18 | laser_geometry 19 | message_generation 20 | visualization_msgs 21 | ) 22 | 23 | find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | ## Define custom messages 26 | #add_message_files( 27 | # FILES 28 | #) 29 | 30 | ## Generate added messages and services with any dependencies listed here 31 | generate_messages( 32 | DEPENDENCIES 33 | std_msgs 34 | ) 35 | 36 | ## Define catkin exports 37 | catkin_package( 38 | # INCLUDE_DIRS include 39 | # LIBRARIES sabertooth_usb 40 | CATKIN_DEPENDS pcl_conversions pcl_ros geometry_msgs trajectory_msgs roscpp rospy std_msgs sensor_msgs message_generation 41 | DEPENDS Boost system_lib 42 | ) 43 | 44 | 45 | include_directories( 46 | include 47 | ${Boost_INCLUDE_DIRS} 48 | ${catkin_INCLUDE_DIRS} 49 | ${Eigen_INCLUDE_DIRS} 50 | ) 51 | 52 | add_definitions(-std=c++11) 53 | 54 | 55 | #add_library( 56 | # 57 | #) 58 | 59 | add_executable(safety_control_node src/safety_control_node.cpp) 60 | 61 | ## Add cmake target dependencies of the executable 62 | ## same as for the library above 63 | add_dependencies(safety_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 64 | 65 | ## Specify libraries to link a library or executable target against 66 | target_link_libraries(safety_control_node 67 | ${catkin_LIBRARIES} 68 | ) 69 | 70 | ############# 71 | ## Install ## 72 | ############# 73 | 74 | # all install targets should use catkin DESTINATION variables 75 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 76 | 77 | ## Mark executable scripts (Python etc.) for installation 78 | ## in contrast to setup.py, you can choose the destination 79 | # install(PROGRAMS 80 | # scripts/my_python_script 81 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 82 | # ) 83 | 84 | ## Mark executables and/or libraries for installation 85 | # install(TARGETS sabertooth_usb sabertooth_usb_node 86 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 88 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 89 | # ) 90 | 91 | ## Mark cpp header files for installation 92 | # install(DIRECTORY include/${PROJECT_NAME}/ 93 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 94 | # FILES_MATCHING PATTERN "*.h" 95 | # PATTERN ".svn" EXCLUDE 96 | # ) 97 | 98 | ## Mark other files for installation (e.g. launch and bag files, etc.) 99 | # install(FILES 100 | # # myfile1 101 | # # myfile2 102 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 103 | # ) 104 | 105 | ############# 106 | ## Testing ## 107 | ############# 108 | 109 | ## Add gtest based cpp test target and link libraries 110 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sabertooth_usb.cpp) 111 | # if(TARGET ${PROJECT_NAME}-test) 112 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 113 | # endif() 114 | 115 | ## Add folders to be run by python nosetests 116 | # catkin_add_nosetests(test) 117 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Safety_control 2 | 3 | [![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) 4 | [![Build Status](https://travis-ci.org/shinselrobots/safety_control.svg?branch=master)](https://travis-ci.org/shinselrobots/safety_control) 5 | 6 | # Info 7 | This node provides basic collision / cliff detection and prevention 8 | To simplify behavior, potential collisions are detected by "zones" around the robot 9 | as defined in SafetySensorSummary 10 | 11 | # Hints 12 | - Tune how long the robot stays stopped by modifying "timeout" in param/cmd_vel_mux.yaml 13 | - Make sure you remap /cmd_vel in your launch file 14 | - Tune for robot using parameters shown in the launch file. 15 | - To view model in RVIZ, add topic "safety_control/zone_marker". This will display a marker 16 | whenever an object is in one of the robot's danger zones 17 | 18 | # Prerequisites 19 | (there may be a better way to add these packages...?) 20 | - modify "cmd_vel_mux.yaml" to add this safety controller: 21 | name: "Safety Control" 22 | topic: "safety_control" 23 | timeout: 1.0 24 | priority: 5 25 | 26 | - Install required packages: 27 | Use the following command to install the required build and runtime dependencies: 28 | ```bash 29 | cd 30 | rosdep install --from-paths `pwd`/src --ignore-src --rosdistro=kinetic -y 31 | ``` 32 | 33 | -------------------------------------------------------------------------------- /launch/dbg_safety_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/safety_control.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 41 | 42 | 43 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | safety_control 4 | 0.0.5 5 | Node to monitor potential collision or cliffs 6 | Dave Shinsel 7 | Apache-2 8 | 9 | catkin 10 | 11 | pcl_conversions 12 | pcl_ros 13 | geometry_msgs 14 | trajectory_msgs 15 | roscpp 16 | rospy 17 | std_msgs 18 | sensor_msgs 19 | message_generation 20 | tf 21 | nav_msgs 22 | laser_geometry 23 | visualization_msgs 24 | 25 | pcl_conversions 26 | pcl_ros 27 | geometry_msgs 28 | trajectory_msgs 29 | roscpp 30 | rospy 31 | std_msgs 32 | sensor_msgs 33 | message_generation 34 | tf 35 | nav_msgs 36 | laser_geometry 37 | pointcloud_to_laserscan 38 | visualization_msgs 39 | 40 | 41 | -------------------------------------------------------------------------------- /src/safety_control_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Dave Shinsel 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | /* Safety Control Node 17 | This node provides basic collision / cliff detection and prevention 18 | To simplify behavior, potential collisions are detected by "zones" around the robot 19 | as defined in SafetySensorSummary 20 | 21 | - Tune how long the robot stays stopped by modifying "timeout" in param/cmd_vel_mux.yaml 22 | - Make sure you remap /cmd_vel in your launch file 23 | */ 24 | 25 | #include 26 | #include "ros/ros.h" 27 | #include "std_msgs/Float32.h" 28 | #include "std_msgs/Empty.h" 29 | #include "std_msgs/String.h" 30 | #include "geometry_msgs/Twist.h" 31 | #include "geometry_msgs/Point32.h" 32 | #include "geometry_msgs/Vector3.h" 33 | #include "sensor_msgs/BatteryState.h" 34 | #include "tf/transform_broadcaster.h" 35 | #include "nav_msgs/Odometry.h" 36 | #include "boost/lexical_cast.hpp" 37 | #include 38 | #include 39 | #include // std::min 40 | #include // std::exception 41 | 42 | // LASER LISTENER 43 | #include "tf/transform_listener.h" 44 | #include "sensor_msgs/PointCloud.h" 45 | #include "tf/message_filter.h" 46 | #include "message_filters/subscriber.h" 47 | #include "laser_geometry/laser_geometry.h" 48 | 49 | 50 | // DIRECT POINTCLOUD 51 | #include 52 | #include 53 | #include 54 | //#include // for testing only 55 | #include 56 | #include 57 | // NOTE: you must install TF2 Sensor Messages: sudo apt-get install ros-kinetic-tf2-sensor-msgs 58 | 59 | //#include 60 | 61 | // ROS REP103: "By the right hand rule, the yaw component of orientation increases 62 | // as the child frame rotates counter-clockwise, 63 | // and for geographic poses, yaw is zero when pointing east." (left is positive, right is negative) 64 | 65 | 66 | enum ZONES { 67 | LEFT_REAR_ZONE = 0, 68 | LEFT_SIDE_ZONE, 69 | LEFT_FRONT_SIDE_ZONE, // In front of robot, to the side 70 | LEFT_FRONT_ZONE, 71 | RIGHT_FRONT_ZONE, 72 | RIGHT_FRONT_SIDE_ZONE, // In front of robot, to the side 73 | RIGHT_SIDE_ZONE, 74 | RIGHT_REAR_ZONE 75 | }; 76 | #define NUMBER_OF_ZONES 8 77 | 78 | enum FIELDS { 79 | LIDAR = 0, 80 | FRONT_DEPTH_CAMERA, 81 | REAR_DEPTH_CAMERA, 82 | FRONT_CLIFF, // track cliffs separately 83 | REAR_CLIFF, 84 | CONFIDENCE, // overall confidence of threat 85 | }; 86 | #define NUMBER_OF_FIELDS 6 87 | 88 | typedef struct 89 | { 90 | float ax; // bottom right corner 91 | float ay; 92 | float bx; // top left corner 93 | float by; 94 | } BOUNDING_BOX_T; 95 | 96 | // Global 97 | const int DEFAULT_LIDAR_CONFIDENCE_THRESHOLD = 4; // number of lidar points needed to indicate object 98 | const int DEFAULT_DEPTH_CAMERA_CONFIDENCE_THRESHOLD = 50; // number of depth camera points to indicate object 99 | const int DEFAULT_CLIFF_CONFIDENCE_THRESHOLD = 150; // number of depth camera points to indicate cliff (large to avoid noise) 100 | const int DEFAULT_OVERALL_CONFIDENCE_THRESHOLD = 4; // number of combined points needed to indicate object 101 | 102 | 103 | //////////////////////////////////////////////////////////////////////////////////////////////////////// 104 | class SafetySensorSummary 105 | { 106 | public: 107 | SafetySensorSummary(); // Constructor automatically initializes defaults 108 | void Increment(int zone, int field); 109 | void SetValue(int zone, int field, int value); 110 | int GetValue(int zone, int field); 111 | void SetConfidence(int field, double confidence_threshold); 112 | bool ThreatDetectedInZone(int zone); 113 | void ClearSensorReadings(int sensor); 114 | void UpdateConfidence(); // Summary of all sensor readings. called after any sensor updates 115 | 116 | int zone_status_[NUMBER_OF_ZONES][NUMBER_OF_FIELDS]; 117 | double sensor_threshold_[NUMBER_OF_FIELDS]; 118 | 119 | }; 120 | 121 | 122 | SafetySensorSummary::SafetySensorSummary() 123 | { 124 | ROS_INFO("SafetySensorSummary: Starting... "); 125 | 126 | sensor_threshold_[LIDAR] = DEFAULT_LIDAR_CONFIDENCE_THRESHOLD; 127 | sensor_threshold_[FRONT_DEPTH_CAMERA] = DEFAULT_DEPTH_CAMERA_CONFIDENCE_THRESHOLD; 128 | sensor_threshold_[REAR_DEPTH_CAMERA] = DEFAULT_DEPTH_CAMERA_CONFIDENCE_THRESHOLD; 129 | sensor_threshold_[FRONT_CLIFF] = DEFAULT_CLIFF_CONFIDENCE_THRESHOLD; 130 | sensor_threshold_[REAR_CLIFF] = DEFAULT_CLIFF_CONFIDENCE_THRESHOLD; 131 | sensor_threshold_[CONFIDENCE] = DEFAULT_OVERALL_CONFIDENCE_THRESHOLD; 132 | 133 | for(int zone=0; zone sensor_threshold_[LIDAR]) 161 | { 162 | lidar_confidence = zone_status_[zone][LIDAR]; 163 | } 164 | 165 | if( zone_status_[zone][FRONT_DEPTH_CAMERA] > sensor_threshold_[FRONT_DEPTH_CAMERA]) 166 | { 167 | front_depth_camera_confidence = zone_status_[zone][FRONT_DEPTH_CAMERA]; 168 | } 169 | 170 | if( zone_status_[zone][REAR_DEPTH_CAMERA] > sensor_threshold_[REAR_DEPTH_CAMERA]) 171 | { 172 | rear_depth_camera_confidence = zone_status_[zone][REAR_DEPTH_CAMERA]; 173 | } 174 | 175 | // CLIFFS 176 | if( zone_status_[zone][FRONT_CLIFF] > sensor_threshold_[FRONT_CLIFF]) 177 | { 178 | front_cliff_confidence = zone_status_[zone][FRONT_CLIFF]; 179 | } 180 | if( zone_status_[zone][REAR_CLIFF] > sensor_threshold_[REAR_CLIFF]) 181 | { 182 | rear_cliff_confidence = zone_status_[zone][REAR_CLIFF]; 183 | } 184 | 185 | zone_status_[zone][CONFIDENCE] = lidar_confidence + 186 | front_depth_camera_confidence + rear_depth_camera_confidence + 187 | front_cliff_confidence + rear_cliff_confidence; 188 | 189 | /* 190 | printf ("DBG UpdateConfidence: For Zone = %d, confidence = %d\n", zone, zone_status_[zone][CONFIDENCE]); 191 | printf (" LIDAR = %4d\n", zone_status_[zone][LIDAR]); 192 | printf (" FRONT_DEPTH_CAMERA = %4d\n", zone_status_[zone][FRONT_DEPTH_CAMERA]); 193 | printf (" REAR_DEPTH_CAMERA = %4d\n", zone_status_[zone][REAR_DEPTH_CAMERA]); 194 | */ 195 | 196 | } 197 | } 198 | 199 | void SafetySensorSummary::SetConfidence(int field, double confidence_threshold) 200 | { 201 | if(field < NUMBER_OF_FIELDS) 202 | { 203 | sensor_threshold_[field] = confidence_threshold; 204 | ROS_INFO("SafetySensorSummary: Confidence for sensor %d changed to %f", field, confidence_threshold); 205 | } 206 | else 207 | { 208 | ROS_ERROR("SafetySensorSummary::SetConfidence: BAD FIELD NUMBER! (%d)", field); 209 | } 210 | } 211 | 212 | void SafetySensorSummary::Increment(int zone, int field) 213 | { 214 | zone_status_[zone][field]++; 215 | } 216 | 217 | void SafetySensorSummary::SetValue(int zone, int field, int value) 218 | { 219 | zone_status_[zone][field] = value; 220 | } 221 | 222 | int SafetySensorSummary::GetValue(int zone, int field) 223 | { 224 | return zone_status_[zone][field]; 225 | } 226 | 227 | bool SafetySensorSummary::ThreatDetectedInZone(int zone) 228 | { 229 | // determine if there is an object in the zone 230 | //printf ("DBG ThreatDetectedInZone: Zone = %d, confidence = %d\n", zone, zone_status_[zone][CONFIDENCE]); 231 | 232 | if( zone_status_[zone][CONFIDENCE] >= sensor_threshold_[CONFIDENCE] ) 233 | { 234 | printf ("DBG OBJECT IN ZONE: Zone = %d, confidence = %d\n", zone, zone_status_[zone][CONFIDENCE]); 235 | printf (" LIDAR = %d, FRONT_DEPTH_CAMERA = %d, FRONT_CLIFF = %d\n", 236 | zone_status_[zone][LIDAR], zone_status_[zone][FRONT_DEPTH_CAMERA], 237 | zone_status_[zone][FRONT_CLIFF]); 238 | 239 | return true; 240 | } 241 | else 242 | { 243 | return false; 244 | } 245 | } 246 | 247 | //////////////////////////////////////////////////////////////////////////////////////////////////////// 248 | class SafetyControl 249 | { 250 | public: 251 | SafetyControl(ros::NodeHandle n); 252 | 253 | private: 254 | 255 | // FUNCTIONS 256 | void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in); 257 | void frontDepthScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in); 258 | void rearDepthScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in); 259 | void odomCallback(const nav_msgs::Odometry::ConstPtr& msg); 260 | //void cmdVelMuxCallback(const geometry_msgs::Twist::ConstPtr& msg); 261 | void DisplayCollisionMarker(int zone); 262 | void UpdateZonesWithSensor(int sensor_type, float x, float y, float z=0.0); 263 | bool IsInBoundingBox(BOUNDING_BOX_T bb, float x, float y); 264 | void HandleThreats(bool threat_detected); 265 | void CheckForThreat(); 266 | void Stop(); 267 | void Turn(float turnh_amount); 268 | bool robotIsMoving(); 269 | 270 | //void cloud_cb (const pcl::PCLPointCloud2ConstPtr& cloud_in); 271 | void frontDepthCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_in); 272 | 273 | // CONSTANTS 274 | 275 | const char* DEFAULT_TARGET_FRAME = "base_link"; // TF frame for sensors 276 | const double DEFAULT_TF_TOLERANCE = 0.01; // TF latency tolerance 277 | 278 | const float DEFAULT_ROBOT_LENGTH = 0.4; // meters 279 | const float DEFAULT_ROBOT_WIDTH = 0.4; 280 | 281 | const float DEFAULT_FRONT_ZONE_RANGE_MIN = 0.2; 282 | const float DEFAULT_FRONT_ZONE_RANGE_SPEED_SCALER = 0.2; 283 | const float DEFAULT_FRONT_ZONE_CLIFF_ADDER = 0.2; // Add extra buffer for cliffs! 284 | const float DEFAULT_FRONT_ZONE_WIDTH = (DEFAULT_ROBOT_WIDTH * 0.75) / 2.0; // a little narrower than half the robot 285 | const float DEFAULT_SIDE_ZONE_RANGE = 0.05; 286 | const float DEFAULT_FRONT_SIDE_ZONE_WIDTH = ((DEFAULT_ROBOT_WIDTH / 2.0) - DEFAULT_FRONT_ZONE_WIDTH) + DEFAULT_SIDE_ZONE_RANGE; 287 | 288 | const float DEFAULT_REAR_ZONE_RANGE_MIN = 0.1; 289 | const float DEFAULT_REAR_ZONE_RANGE_SPEED_SCALER = 0.2; 290 | const float DEFAULT_REAR_ZONE_CLIFF_ADDER = 0.2; // Add extra buffer for cliffs! 291 | const float DEFAULT_REAR_ZONE_WIDTH = DEFAULT_ROBOT_WIDTH / 2.0; 292 | 293 | const float DEFAULT_AVOID_TURN_AMOUNT = 0.8; // radians per second 294 | const float DEFAULT_OBJECT_MIN_HEIGHT = 0.0508; // avoid any objects higher than this (2 inches) 295 | const float DEFAULT_OBJECT_MAX_HEIGHT = 1.0; // ignore any objects taller than the robot (using 1 meter for now) 296 | const float DEFAULT_CLIFF_MIN_HEIGHT = -0.03; //-0.0508; // find cliffs bigger than this (2 inches) 297 | const float DEFAULT_CLIFF_MAX_HEIGHT = -1.0; // sensor reading larger than this is probably an error 298 | 299 | // PARAMETERS 300 | 301 | std::float_t robot_length_; 302 | std::float_t robot_width_; 303 | 304 | std::float_t front_zone_range_min_; 305 | std::float_t front_zone_range_speed_scaler_; 306 | std::float_t front_zone_cliff_adder_; 307 | std::float_t front_zone_width_; 308 | 309 | std::float_t side_zone_range_; 310 | std::float_t front_side_zone_width_; 311 | 312 | std::float_t rear_zone_range_min_; 313 | std::float_t rear_zone_range_speed_scaler_; 314 | std::float_t rear_zone_cliff_adder_; 315 | std::float_t rear_zone_width_; 316 | 317 | std::float_t avoid_turn_amount_; 318 | 319 | std::float_t object_min_height_; // find any objects higher than this 320 | std::float_t object_max_height_; // ignore any objects taller than the robot (using 1 meter for now) 321 | std::float_t cliff_min_height_; // find cliffs higher than this 322 | std::float_t cliff_max_height_; // ignore any cliffs taller than this, probably an error 323 | 324 | 325 | // VARIABLES 326 | 327 | ros::NodeHandle nh_; 328 | ros::NodeHandle private_nh_; 329 | 330 | std::string target_frame_; 331 | double tf_tolerance_; 332 | tf2_ros::Buffer tf2_; 333 | tf2_ros::TransformListener tfListener_; 334 | 335 | sensor_msgs::PointCloud laser_scan_cloud_; 336 | laser_geometry::LaserProjection laser_scan_projector_; 337 | tf::TransformListener laser_scan_listener_; 338 | 339 | sensor_msgs::PointCloud front_depth_scan_cloud_; 340 | laser_geometry::LaserProjection front_depth_scan_projector_; 341 | tf::TransformListener front_depth_scan_listener_; 342 | 343 | sensor_msgs::PointCloud rear_depth_scan_cloud_; 344 | laser_geometry::LaserProjection rear_depth_scan_projector_; 345 | tf::TransformListener rear_depth_scan_listener_; 346 | 347 | SafetySensorSummary sensor_summary_; 348 | float odom_linear_velocity_; // current velocity as reported by odometry 349 | float odom_angular_velocity_; 350 | //float cmd_mux_linear_velocity_; // current speed requested by the mux 351 | //float cmd_mux_angular_velocity_; // (for whatever module has the mux) 352 | float front_zone_range_; 353 | float rear_zone_range_; 354 | 355 | 356 | 357 | // SUBSCRIBERS 358 | message_filters::Subscriber laser_sub; // from lidar 359 | tf::MessageFilter laser_notifier; 360 | 361 | message_filters::Subscriber front_depth_scan_sub_; // from front depth camera 362 | tf::MessageFilter front_depth_scan_notifier_; 363 | 364 | message_filters::Subscriber rear_depth_scan_sub_; // from front depth camera 365 | tf::MessageFilter rear_depth_scan_notifier_; 366 | 367 | ros::Subscriber odom_sub_; 368 | ros::Subscriber cmd_vel_sub_; // current velocity being requested by whatever node has the mux 369 | ros::Subscriber front_depth_cloud_sub_; 370 | 371 | // PUBLISHERS 372 | ros::Publisher laser_scan_pub_; 373 | ros::Publisher front_depth_scan_pub_; 374 | ros::Publisher rear_depth_scan_pub_; 375 | ros::Publisher marker_pub_; 376 | ros::Publisher cmd_vel_pub_; 377 | //ros::Publisher voxel_pub_; 378 | 379 | 380 | }; 381 | //////////////////////////////////////////////////////////////////////////////////////////////////////// 382 | 383 | 384 | SafetyControl::SafetyControl(ros::NodeHandle n) : 385 | nh_(n), 386 | private_nh_("~"), 387 | laser_sub(nh_, "/scan", 10), 388 | laser_notifier(laser_sub,laser_scan_listener_, "base_link", 10), 389 | 390 | front_depth_scan_sub_(nh_, "/camera/scan1", 10), // from front depth camera 391 | front_depth_scan_notifier_(front_depth_scan_sub_, front_depth_scan_listener_, "base_link", 10), 392 | 393 | rear_depth_scan_sub_(nh_, "/camera/scan2", 10), // from front depth camera ==> DAVES TODO - FIX TOPIC!! 394 | rear_depth_scan_notifier_(rear_depth_scan_sub_, rear_depth_scan_listener_, "base_link", 10), 395 | 396 | tfListener_(tf2_), 397 | 398 | odom_linear_velocity_(0.0), 399 | odom_angular_velocity_(0.0) 400 | 401 | { 402 | 403 | ROS_INFO("SafetyControl: Initializing..."); 404 | 405 | 406 | 407 | // PARAMETERS 408 | printf("\nSETTABLE PARAMS: \n"); 409 | 410 | private_nh_.param("target_frame", target_frame_, DEFAULT_TARGET_FRAME); // TF frame for sensors 411 | private_nh_.param("transform_tolerance", tf_tolerance_, DEFAULT_TF_TOLERANCE); 412 | 413 | private_nh_.param("robot_length", robot_length_, DEFAULT_ROBOT_LENGTH); 414 | private_nh_.param("robot_width", robot_width_, DEFAULT_ROBOT_WIDTH); 415 | 416 | private_nh_.param("front_zone_range_min", front_zone_range_min_, DEFAULT_FRONT_ZONE_RANGE_MIN); 417 | private_nh_.param("front_zone_range_speed_scaler", front_zone_range_speed_scaler_, DEFAULT_FRONT_ZONE_RANGE_SPEED_SCALER); 418 | private_nh_.param("front_zone_cliff_adder", front_zone_cliff_adder_, DEFAULT_FRONT_ZONE_CLIFF_ADDER); 419 | private_nh_.param("front_zone_width", front_zone_width_, DEFAULT_FRONT_ZONE_WIDTH); 420 | 421 | private_nh_.param("side_zone_range", side_zone_range_, DEFAULT_SIDE_ZONE_RANGE); 422 | private_nh_.param("front_side_zone_width", front_side_zone_width_, DEFAULT_FRONT_SIDE_ZONE_WIDTH); 423 | 424 | private_nh_.param("rear_zone_range_min", rear_zone_range_min_, DEFAULT_REAR_ZONE_RANGE_MIN); 425 | private_nh_.param("rear_zone_range_speed_scaler", rear_zone_range_speed_scaler_, DEFAULT_REAR_ZONE_RANGE_SPEED_SCALER); 426 | private_nh_.param("rear_zone_cliff_adder", rear_zone_cliff_adder_, DEFAULT_REAR_ZONE_CLIFF_ADDER); 427 | 428 | private_nh_.param("rear_zone_width", rear_zone_width_, DEFAULT_REAR_ZONE_WIDTH); 429 | 430 | private_nh_.param("avoid_turn_amount", avoid_turn_amount_, DEFAULT_AVOID_TURN_AMOUNT); 431 | private_nh_.param("object_min_height", object_min_height_, DEFAULT_OBJECT_MIN_HEIGHT); 432 | private_nh_.param("object_max_height", object_max_height_, DEFAULT_OBJECT_MAX_HEIGHT); 433 | private_nh_.param("cliff_min_height", cliff_min_height_, DEFAULT_CLIFF_MIN_HEIGHT); 434 | private_nh_.param("cliff_max_height", cliff_max_height_, DEFAULT_CLIFF_MAX_HEIGHT); 435 | 436 | // initialize variable sensor ranges 437 | front_zone_range_ = front_zone_range_min_; // robot not moving initially 438 | rear_zone_range_ = rear_zone_range_min_; 439 | 440 | 441 | // Display current settings 442 | printf(" target_frame: %s (default = '%s')\n", target_frame_.c_str(), DEFAULT_TARGET_FRAME); 443 | printf(" tf_tolerance: %f (default = %f)\n\n", tf_tolerance_, DEFAULT_TF_TOLERANCE); 444 | 445 | printf(" robot_length: %f (default = %f)\n", robot_length_, DEFAULT_ROBOT_LENGTH); 446 | printf(" robot_width: %f (default = %f)\n\n", robot_width_, DEFAULT_ROBOT_WIDTH); 447 | 448 | printf(" front_zone_range_min: %f (default = %f)\n", front_zone_range_min_, DEFAULT_FRONT_ZONE_RANGE_MIN); 449 | printf(" front_zone_range_speed_scaler: %f (default = %f)\n", front_zone_range_speed_scaler_, DEFAULT_FRONT_ZONE_RANGE_SPEED_SCALER); 450 | printf(" front_zone_cliff_adder: %f (default = %f)\n", front_zone_cliff_adder_, DEFAULT_FRONT_ZONE_CLIFF_ADDER); 451 | printf(" front_zone_width: %f (default = %f)\n\n", front_zone_width_, DEFAULT_FRONT_ZONE_WIDTH); 452 | 453 | printf(" side_zone_range: %f (default = %f)\n", side_zone_range_, DEFAULT_SIDE_ZONE_RANGE); 454 | printf(" front_side_zone_width: %f (default = %f)\n\n", front_side_zone_width_, DEFAULT_FRONT_SIDE_ZONE_WIDTH); 455 | 456 | printf(" rear_zone_range_min: %f (default = %f)\n", rear_zone_range_min_, DEFAULT_REAR_ZONE_RANGE_MIN); 457 | printf(" rear_zone_range_speed_scaler: %f (default = %f)\n", rear_zone_range_speed_scaler_, DEFAULT_REAR_ZONE_RANGE_SPEED_SCALER); 458 | printf(" rear_zone_cliff_adder: %f (default = %f)\n", rear_zone_cliff_adder_, DEFAULT_REAR_ZONE_CLIFF_ADDER); 459 | printf(" rear_zone_width: %f (default = %f)\n\n", rear_zone_width_, DEFAULT_REAR_ZONE_WIDTH); 460 | 461 | printf(" avoid_turn_amount: %f (default = %f)\n", avoid_turn_amount_, DEFAULT_AVOID_TURN_AMOUNT); 462 | printf(" object_min_height: %f (default = %f)\n", object_min_height_, DEFAULT_OBJECT_MIN_HEIGHT); 463 | printf(" object_max_height: %f (default = %f)\n", object_max_height_, DEFAULT_OBJECT_MAX_HEIGHT); 464 | printf(" cliff_min_height: %f (default = %f)\n", cliff_min_height_, DEFAULT_CLIFF_MIN_HEIGHT); 465 | printf(" cliff_max_height: %f (default = %f)\n\n", cliff_max_height_, DEFAULT_CLIFF_MAX_HEIGHT); 466 | 467 | 468 | printf(" THRESHOLDS:\n"); 469 | 470 | // set optional parameters for the SensorSummary class 471 | int threshold; 472 | 473 | if ( private_nh_.param("lidar_confidence_threshold", threshold, DEFAULT_LIDAR_CONFIDENCE_THRESHOLD) ) 474 | { 475 | sensor_summary_.SetConfidence(LIDAR, threshold); 476 | } 477 | printf(" lidar_confidence_threshold: %4d (default = %4d)\n", 478 | threshold, DEFAULT_LIDAR_CONFIDENCE_THRESHOLD); 479 | 480 | if ( private_nh_.param("depth_camera_confidence_threshold", threshold, DEFAULT_DEPTH_CAMERA_CONFIDENCE_THRESHOLD) ) 481 | { 482 | sensor_summary_.SetConfidence(FRONT_DEPTH_CAMERA, threshold); 483 | sensor_summary_.SetConfidence(REAR_DEPTH_CAMERA, threshold); 484 | } 485 | printf(" depth_camera_confidence_threshold: %4d (default = %4d)\n", 486 | threshold, DEFAULT_LIDAR_CONFIDENCE_THRESHOLD); 487 | 488 | if ( private_nh_.param("cliff_confidence_threshold", threshold, DEFAULT_CLIFF_CONFIDENCE_THRESHOLD) ) 489 | { 490 | sensor_summary_.SetConfidence(FRONT_CLIFF, threshold); 491 | sensor_summary_.SetConfidence(REAR_CLIFF, threshold); 492 | } 493 | printf(" cliff_confidence_threshold: %4d (default = %4d)\n", 494 | threshold, DEFAULT_CLIFF_CONFIDENCE_THRESHOLD); 495 | 496 | if ( private_nh_.param("overall_confidence_threshold", threshold, DEFAULT_OVERALL_CONFIDENCE_THRESHOLD) ) 497 | { 498 | sensor_summary_.SetConfidence(CONFIDENCE, threshold); 499 | } 500 | printf(" overall_confidence_threshold: %4d (default = %4d)\n\n", 501 | threshold, DEFAULT_OVERALL_CONFIDENCE_THRESHOLD); 502 | 503 | 504 | 505 | 506 | // SUBSCRIBERS 507 | laser_notifier.registerCallback( 508 | boost::bind(&SafetyControl::laserScanCallback, this, _1)); 509 | laser_notifier.setTolerance(ros::Duration(0.02)); 510 | 511 | front_depth_scan_notifier_.registerCallback( 512 | boost::bind(&SafetyControl::frontDepthScanCallback, this, _1)); 513 | front_depth_scan_notifier_.setTolerance(ros::Duration(0.01)); 514 | 515 | rear_depth_scan_notifier_.registerCallback( 516 | boost::bind(&SafetyControl::rearDepthScanCallback, this, _1)); 517 | rear_depth_scan_notifier_.setTolerance(ros::Duration(0.01)); 518 | 519 | // DAVES - DEPTH CLOUD 520 | front_depth_cloud_sub_ = nh_.subscribe ("/camera/points", 1, &SafetyControl::frontDepthCloudCallback, this); 521 | 522 | 523 | odom_sub_ = nh_.subscribe("odom", 1000, &SafetyControl::odomCallback, this); 524 | //cmd_vel_sub_ = nh_.subscribe("/mobile_base/commands/velocity", 1000, &SafetyControl::cmdVelMuxCallback, this); 525 | 526 | 527 | // PUBLISHERS 528 | laser_scan_pub_ = nh_.advertise("safety_control/laser_cloud",1); 529 | front_depth_scan_pub_ = nh_.advertise("safety_control/laser_cloud2",1); 530 | rear_depth_scan_pub_ = nh_.advertise("safety_control/laser_cloud3",1); 531 | 532 | marker_pub_ = nh_.advertise("safety_control/zone_marker", 1); 533 | cmd_vel_pub_ = nh_.advertise("/cmd_vel", 1); 534 | 535 | //voxel_pub_ = nh_.advertise ("output", 1); // DAVES TODO - set correct message! 536 | 537 | ROS_INFO("SafetyControl: listening for topics: /scan, odom, /camera/points "); 538 | ROS_INFO("SafetyControl: publishing topics: /cmd_vel, safety_control/zone_marker, safety_control/laser_cloud "); 539 | 540 | } 541 | 542 | 543 | //////////////////////////////////////////////////////////////////////////////////////////////////////// 544 | 545 | void SafetyControl::frontDepthCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) 546 | { 547 | // ROS_INFO("DBG: Got FRONT DEPTH CLOUD"); 548 | 549 | sensor_msgs::PointCloud2ConstPtr cloud_out; 550 | sensor_msgs::PointCloud2Ptr cloud; 551 | 552 | try 553 | { 554 | cloud.reset(new sensor_msgs::PointCloud2); 555 | tf2_.transform(*cloud_msg, *cloud, target_frame_, ros::Duration(tf_tolerance_)); 556 | cloud_out = cloud; 557 | } 558 | catch (tf2::TransformException &ex) 559 | { 560 | // ROS_WARN_STREAM("Transform failure: " << ex.what()); 561 | return; 562 | } 563 | 564 | int total_points_found = 0; 565 | int points_in_boundry = 0; 566 | int cliff_points_found = 0; 567 | double dbg_total_to_average = 0.0; 568 | 569 | // clear prior readings 570 | sensor_summary_.ClearSensorReadings(FRONT_DEPTH_CAMERA); 571 | sensor_summary_.ClearSensorReadings(FRONT_CLIFF); 572 | 573 | // Iterate through pointcloud 574 | for (sensor_msgs::PointCloud2ConstIterator 575 | iter_x(*cloud, "x"), iter_y(*cloud, "y"), iter_z(*cloud, "z"); 576 | iter_x != iter_x.end(); 577 | ++iter_x, ++iter_y, ++iter_z) 578 | { 579 | 580 | 581 | // Discard any NAN points 582 | if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z)) 583 | { 584 | // ROS_INFO("rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z); 585 | continue; 586 | } 587 | total_points_found++; 588 | dbg_total_to_average += *iter_z; 589 | 590 | 591 | // look for objects and cliffs 592 | // Test height of each point, reject those too high or too low 593 | 594 | if (*iter_z < cliff_min_height_ && *iter_z > cliff_max_height_) // negative values (below the floor) 595 | { 596 | // CLIFF Detected 597 | //ROS_INFO("found cliff point: z = %f in range (%f, %f)", *iter_z, cliff_min_height_, cliff_max_height_); 598 | cliff_points_found++; 599 | UpdateZonesWithSensor(FRONT_CLIFF, *iter_x, *iter_y, *iter_z); 600 | 601 | } 602 | else if (*iter_z > object_min_height_ && *iter_z < object_max_height_) 603 | { 604 | // OBJECT Detected 605 | // ROS_INFO("rejected for height %f not in range (%f, %f)", *iter_z, object_min_height_, object_max_height_); 606 | points_in_boundry++; 607 | UpdateZonesWithSensor(FRONT_DEPTH_CAMERA, *iter_x, *iter_y, *iter_z); 608 | } 609 | } 610 | 611 | if( total_points_found != 0 ) 612 | { 613 | double dbg_average = dbg_total_to_average/total_points_found; 614 | ROS_INFO(">>> total points found = %d, averageZ = %f", total_points_found, dbg_average ); 615 | } 616 | 617 | //ROS_INFO("DEPTH CLOUD: total points found = %d, in boundry = %d", total_points_found, points_in_boundry); 618 | //ROS_INFO("DEPTH CLOUD: cliff points found = %d", cliff_points_found); 619 | 620 | // update system confidence 621 | sensor_summary_.UpdateConfidence(); 622 | 623 | // See if any objects pose a threat to the robot, and handle them 624 | CheckForThreat(); 625 | 626 | 627 | } 628 | 629 | 630 | void SafetyControl::laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in) 631 | { 632 | 633 | // ROS_INFO("DBG: Got LIDAR Laser Scan"); 634 | try 635 | { 636 | laser_scan_projector_.transformLaserScanToPointCloud( 637 | "base_link",*scan_in, laser_scan_cloud_,laser_scan_listener_); 638 | } 639 | catch (tf::TransformException& e) 640 | { 641 | std::cout << e.what(); 642 | return; 643 | } 644 | 645 | // Got a valid laser scan, which has been converted to a point cloud (X,Y values, z is 0) 646 | // publish the laserCloud for debug purposes 647 | laser_scan_pub_.publish(laser_scan_cloud_); 648 | 649 | 650 | // clear Lidar readings 651 | sensor_summary_.ClearSensorReadings(LIDAR); 652 | 653 | geometry_msgs::Point32 nearX, nearY; 654 | nearX.x = 10000; 655 | nearY.y = 10000; 656 | 657 | 658 | //Update Sensor Summary with LaserScan 659 | uint32_t num_points = laser_scan_cloud_.points.size(); 660 | //printf ("DBG LaserCloud: number points = %d\n", num_points); 661 | for( unsigned int i=0; i < num_points; i++) 662 | { 663 | //printf ("DBG laserScanCallback: x = %f, y = %f\n", laser_scan_cloud_.points[i].x, laser_scan_cloud_.points[i].y); 664 | 665 | if( fabs(laser_scan_cloud_.points[i].x) < fabs(nearX.x)) 666 | { 667 | nearX.x = laser_scan_cloud_.points[i].x; 668 | nearX.y = laser_scan_cloud_.points[i].y; 669 | } 670 | if( fabs(laser_scan_cloud_.points[i].y) < fabs(nearY.y)) 671 | { 672 | nearY.x = laser_scan_cloud_.points[i].x; 673 | nearY.y = laser_scan_cloud_.points[i].y; 674 | } 675 | 676 | UpdateZonesWithSensor(LIDAR, laser_scan_cloud_.points[i].x, laser_scan_cloud_.points[i].y); 677 | } 678 | 679 | //printf ("DBG LaserCloud: \n Nearest X = %f, %f \n Nearest Y = %f, %f\n", 680 | // nearX.x, nearX.y, nearY.x, nearY.y); 681 | 682 | 683 | // update system confidence 684 | sensor_summary_.UpdateConfidence(); 685 | 686 | // See if any objects pose a threat to the robot, and handle them 687 | CheckForThreat(); 688 | 689 | } 690 | 691 | 692 | 693 | 694 | void SafetyControl::frontDepthScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in) 695 | { 696 | // WARNING - for this to work, you must launch "pointcloud_to_laser.launch" 697 | 698 | ROS_INFO("DBG: Got FRONT DEPTH CAMERA Laser Scan"); 699 | try 700 | { 701 | front_depth_scan_projector_.transformLaserScanToPointCloud( 702 | "base_link",*scan_in, front_depth_scan_cloud_,front_depth_scan_listener_); 703 | } 704 | catch (tf::TransformException& e) 705 | { 706 | std::cout << e.what(); 707 | return; 708 | } 709 | 710 | // Got a valid laser scan, which has been converted to a point cloud (X,Y values, z is 0) 711 | // publish the laserCloud for debug purposes 712 | front_depth_scan_pub_.publish(front_depth_scan_cloud_); 713 | 714 | // clear Lidar readings 715 | sensor_summary_.ClearSensorReadings(FRONT_DEPTH_CAMERA); 716 | 717 | //Update Sensor Summary with LaserScan 718 | uint32_t num_points = front_depth_scan_cloud_.points.size(); 719 | // printf ("DBG LaserCloud: number points = %d\n", num_points); 720 | for( unsigned int i=0; i < num_points; i++) 721 | { 722 | UpdateZonesWithSensor(FRONT_DEPTH_CAMERA, front_depth_scan_cloud_.points[i].x, laser_scan_cloud_.points[i].y); 723 | } 724 | 725 | // update system confidence 726 | sensor_summary_.UpdateConfidence(); 727 | 728 | // See if any objects pose a threat to the robot, and handle them 729 | CheckForThreat(); 730 | 731 | 732 | } 733 | 734 | void SafetyControl::rearDepthScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in) 735 | { 736 | ROS_INFO("DBG: Got REAR DEPTH CAMERA Laser Scan"); 737 | try 738 | { 739 | rear_depth_scan_projector_.transformLaserScanToPointCloud( 740 | "base_link",*scan_in, rear_depth_scan_cloud_, rear_depth_scan_listener_); 741 | } 742 | catch (tf::TransformException& e) 743 | { 744 | std::cout << e.what(); 745 | return; 746 | } 747 | 748 | // Got a valid laser scan, which has been converted to a point cloud (X,Y values, z is 0) 749 | // publish the laserCloud for debug purposes 750 | rear_depth_scan_pub_.publish(rear_depth_scan_cloud_); 751 | 752 | // clear Lidar readings 753 | sensor_summary_.ClearSensorReadings(FRONT_DEPTH_CAMERA); 754 | 755 | //Update Sensor Summary with LaserScan 756 | uint32_t num_points = rear_depth_scan_cloud_.points.size(); 757 | // printf ("DBG LaserCloud: number points = %d\n", num_points); 758 | for( unsigned int i=0; i < num_points; i++) 759 | { 760 | UpdateZonesWithSensor(FRONT_DEPTH_CAMERA, rear_depth_scan_cloud_.points[i].x, laser_scan_cloud_.points[i].y); 761 | } 762 | 763 | // update system confidence 764 | sensor_summary_.UpdateConfidence(); 765 | 766 | // See if any objects pose a threat to the robot, and handle them 767 | CheckForThreat(); 768 | 769 | } 770 | 771 | 772 | void SafetyControl::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) 773 | { 774 | // Current velocity reported by Odometry 775 | // ROS_INFO("DBG Odom: Linear = %f, Angular = %f", msg->twist.twist.linear.x, msg->twist.twist.angular.z); 776 | odom_linear_velocity_ = msg->twist.twist.linear.x; 777 | odom_angular_velocity_ = msg->twist.twist.angular.z; 778 | const float velocity_max = 0.4; // approx max speed of turtlebot 779 | 780 | // Set dynamic zone ranges. 781 | front_zone_range_ = front_zone_range_min_; 782 | rear_zone_range_ = rear_zone_range_min_; 783 | if(odom_linear_velocity_ > 0.0) 784 | { 785 | // Robot moving forward 786 | front_zone_range_ = front_zone_range_min_ + ((odom_linear_velocity_ / velocity_max) * front_zone_range_speed_scaler_); 787 | } 788 | else if(odom_linear_velocity_ < 0.0) 789 | { 790 | // Robot backing up 791 | rear_zone_range_ = rear_zone_range_min_ - ((odom_linear_velocity_ / velocity_max) * rear_zone_range_speed_scaler_); 792 | } 793 | 794 | //ROS_INFO("DBG Odom-> front_zone_range_: [%f], rear_zone_range_: [%f]", front_zone_range_, rear_zone_range_); 795 | 796 | } 797 | 798 | /* 799 | void SafetyControl::cmdVelMuxCallback(const geometry_msgs::Twist::ConstPtr& msg) 800 | { 801 | // Current velocity being requested by the mux 802 | //ROS_INFO("VelMux-> Linear: [%f], Angular: [%f]", msg->linear.x, msg->angular.z); 803 | cmd_mux_linear_velocity_ = msg->linear.x; 804 | cmd_mux_angular_velocity_ = msg->angular.z; 805 | 806 | } 807 | */ 808 | 809 | 810 | void SafetyControl::Stop() 811 | { 812 | // send high priority message to motor control overriding most behaviors 813 | geometry_msgs::Twist cmd; 814 | cmd.linear.x = cmd.angular.z = 0; 815 | cmd_vel_pub_.publish(cmd); 816 | 817 | } 818 | 819 | 820 | void SafetyControl::Turn(float turn_amount) 821 | { 822 | // send high priority message to motor control overriding most behaviors 823 | geometry_msgs::Twist cmd; 824 | cmd.linear.x = 0.0; // force pivot turn. (Cant continue at current speed, becuase safety control takes over the mux!) 825 | cmd.angular.z = turn_amount; // but turn as needed 826 | cmd_vel_pub_.publish(cmd); 827 | 828 | } 829 | 830 | 831 | void SafetyControl::CheckForThreat() 832 | { 833 | 834 | // Check for Collisions or Cliffs, and display in RVIZ 835 | bool threat_detected = false; 836 | for(int zone_number=0; zone_number 0.0) 899 | { 900 | Stop(); // block robot from moving forward. Turns are OK. 901 | ROS_INFO("Safety_control: Object ahead, Robot stopping"); 902 | return; 903 | } 904 | 905 | } 906 | else if(sensor_summary_.ThreatDetectedInZone(LEFT_FRONT_ZONE) || sensor_summary_.ThreatDetectedInZone(LEFT_FRONT_SIDE_ZONE) || sensor_summary_.ThreatDetectedInZone(LEFT_SIDE_ZONE)) 907 | { 908 | // object to the left, steer away from it 909 | // if strong enough turn already being requested, do nothing 910 | //if(cmd_mux_angular_velocity_ > (-avoid_turn_amount_)) 911 | { 912 | // stop and turn right 913 | Turn(-avoid_turn_amount_); // RadiansPerSecond 914 | ROS_INFO("Safety_control: Object left, turning right"); 915 | } 916 | } 917 | else if(sensor_summary_.ThreatDetectedInZone(RIGHT_FRONT_ZONE) || sensor_summary_.ThreatDetectedInZone(RIGHT_FRONT_SIDE_ZONE) || sensor_summary_.ThreatDetectedInZone(RIGHT_SIDE_ZONE)) 918 | { 919 | // object to the right, steer away from it 920 | // if strong enough turn already being requested, do nothing 921 | //if(cmd_mux_angular_velocity_ < avoid_turn_amount_) 922 | { 923 | // stop and turn left 924 | Turn(avoid_turn_amount_); // RadiansPerSecond 925 | ROS_INFO("Safety_control: Object right, turning left"); 926 | } 927 | } 928 | 929 | } 930 | 931 | 932 | bool SafetyControl::IsInBoundingBox(BOUNDING_BOX_T bb, float x, float y) 933 | { 934 | // printf ("DBG IsInBoundingBox: bb.bx = %f, bb.ax = %f, bb.by = %f, bb.ay = %f\n", bb.bx, bb.ax, bb.by, bb.ay); 935 | // printf ("DBG IsInBoundingBox: x = %f, y = %f", x, y); 936 | 937 | if( x <= bb.bx && bb.ax <= x && y <= bb.by && bb.ay <= y ) 938 | { 939 | // printf (" TRUE\n"); 940 | return true; 941 | } 942 | else 943 | { 944 | // printf (" FALSE\n"); 945 | return false; 946 | } 947 | } 948 | 949 | void SafetyControl::UpdateZonesWithSensor(int sensor_type, float x, float y, float z) 950 | { 951 | // for each sensor reading (eg., a point in the laser scan), see if it intersects a safety zone. 952 | // add up all the hits (which increase confidence that a hit actually occured) 953 | // Bounding box: (bx,by) are top-left coordinates, (ax,ay) are bottom-right coordinates. 954 | 955 | double front_zone_range = front_zone_range_; 956 | double rear_zone_range = rear_zone_range_; 957 | 958 | // Add additional safety distance for cliffs (it's very bad to fall off a cliff!) 959 | if(FRONT_CLIFF == sensor_type) 960 | { 961 | front_zone_range += front_zone_cliff_adder_; 962 | //printf ("DBG UpdateZonesWithSensor: CLIFF DEBUG: x = %f, y = %f, z = %f\n", x, y, z); 963 | } 964 | else if(REAR_CLIFF == sensor_type) 965 | { 966 | rear_zone_range += rear_zone_cliff_adder_; 967 | } 968 | 969 | // LEFT_FRONT_ZONE: 970 | BOUNDING_BOX_T left_front_zone; 971 | left_front_zone.ax = robot_length_ / 2.0; 972 | left_front_zone.ay = 0.0; 973 | left_front_zone.bx = left_front_zone.ax + front_zone_range; 974 | left_front_zone.by = front_zone_width_; 975 | //printf ("DBG UpdateZonesWithSensor: robot_length_ = %f, front_zone_range = %f\n", robot_length_, front_zone_range); 976 | //printf ("DBG UpdateZonesWithSensor: left_front_zone.bx = %f, left_front_zone.ax = %f, left_front_zone.by = %f, left_front_zone.ay = %f\n", left_front_zone.bx, left_front_zone.ax, left_front_zone.by, left_front_zone.ay); 977 | //printf ("DBG UpdateZonesWithSensor: LEFT_FRONT_ZONE: x = %f, y = %f\n", x, y); 978 | if( IsInBoundingBox(left_front_zone, x, y) ) 979 | { 980 | // point is in the box 981 | // if(FRONT_CLIFF == sensor_type) 982 | //printf (">>>>>> DBG UpdateZonesWithSensor: IN LEFT_FRONT_ZONE: sensor = %d, x = %f, y = %f, z = %f, x-ax = %f\n", sensor_type, x, y, z, (x-left_front_zone.ax) ); 983 | 984 | sensor_summary_.Increment(LEFT_FRONT_ZONE, sensor_type); 985 | } 986 | 987 | // RIGHT_FRONT_ZONE 988 | BOUNDING_BOX_T right_front_zone; 989 | right_front_zone.ax = robot_length_ / 2.0; 990 | right_front_zone.ay = -1.0 * front_zone_width_; 991 | right_front_zone.bx = right_front_zone.ax + front_zone_range; 992 | right_front_zone.by = 0.0; 993 | //printf ("DBG UpdateZonesWithSensor: RIGHT_FRONT_ZONE: x = %f, y = %f\n", x, y); 994 | if( IsInBoundingBox(right_front_zone, x, y) ) 995 | { 996 | // point is in the box 997 | //printf ("DBG UpdateZonesWithSensor: IN RIGHT_FRONT_ZONE: x = %f, y = %f\n", x, y); 998 | //printf ("DBG UpdateZonesWithSensor: IN RIGHT_FRONT_ZONE: sensor = %d, x = %f, y = %f, z = %f, x-ax = %f\n", sensor_type, x, y, z, (x-left_front_zone.ax) ); 999 | sensor_summary_.Increment(RIGHT_FRONT_ZONE, sensor_type); 1000 | } 1001 | 1002 | // LEFT_REAR_ZONE: 1003 | BOUNDING_BOX_T left_rear_zone; 1004 | left_rear_zone.bx = -1.0 * robot_length_ / 2.0; 1005 | left_rear_zone.by = rear_zone_width_; 1006 | left_rear_zone.ax = left_rear_zone.bx - rear_zone_range; 1007 | left_rear_zone.ay = 0.0; 1008 | if( IsInBoundingBox(left_rear_zone, x, y) ) 1009 | { 1010 | // point is in the box 1011 | sensor_summary_.Increment(LEFT_REAR_ZONE, sensor_type); 1012 | } 1013 | 1014 | // RIGHT_REAR_ZONE 1015 | BOUNDING_BOX_T right_rear_zone; 1016 | right_rear_zone.bx = -1.0 * robot_length_ / 2.0; 1017 | right_rear_zone.by = 0.0; 1018 | right_rear_zone.ax = right_rear_zone.bx - rear_zone_range; 1019 | right_rear_zone.ay = -1.0 * rear_zone_width_; 1020 | if( IsInBoundingBox(right_rear_zone, x, y) ) 1021 | { 1022 | // point is in the box 1023 | sensor_summary_.Increment(RIGHT_REAR_ZONE, sensor_type); 1024 | } 1025 | 1026 | // LEFT_SIDE_ZONE: 1027 | BOUNDING_BOX_T left_side_zone; 1028 | left_side_zone.ax = -1.0 * robot_length_ / 2.0; // from the back of the robot 1029 | left_side_zone.ay = robot_width_ / 2.0; 1030 | left_side_zone.bx = robot_length_ / 2.0; // to the front of the robot 1031 | left_side_zone.by = left_side_zone.ay + side_zone_range_; 1032 | if( IsInBoundingBox(left_side_zone, x, y) ) 1033 | { 1034 | // point is in the box 1035 | sensor_summary_.Increment(LEFT_SIDE_ZONE, sensor_type); 1036 | } 1037 | 1038 | // RIGHT_SIDE_ZONE: 1039 | BOUNDING_BOX_T right_side_zone; 1040 | right_side_zone.bx = robot_length_ / 2.0; 1041 | right_side_zone.by = -1.0 * robot_width_ / 2.0; 1042 | right_side_zone.ax = -1.0 * robot_length_ / 2.0; 1043 | right_side_zone.ay = right_side_zone.by - side_zone_range_; 1044 | if( IsInBoundingBox(right_side_zone, x, y) ) 1045 | { 1046 | // point is in the box 1047 | sensor_summary_.Increment(RIGHT_SIDE_ZONE, sensor_type); 1048 | } 1049 | 1050 | // LEFT_FRONT_SIDE_ZONE: 1051 | BOUNDING_BOX_T left_front_side_zone; 1052 | left_front_side_zone.ax = robot_length_ / 2.0; 1053 | left_front_side_zone.ay = front_zone_width_; 1054 | left_front_side_zone.bx = left_front_side_zone.ax + front_zone_range; 1055 | left_front_side_zone.by = front_zone_width_ + front_side_zone_width_; 1056 | if( IsInBoundingBox(left_front_side_zone, x, y) ) 1057 | { 1058 | // point is in the box 1059 | sensor_summary_.Increment(LEFT_FRONT_SIDE_ZONE, sensor_type); 1060 | } 1061 | 1062 | // RIGHT_FRONT_SIDE_ZONE 1063 | BOUNDING_BOX_T right_front_side_zone; 1064 | right_front_side_zone.ax = robot_length_ / 2.0; 1065 | right_front_side_zone.ay = -1.0 * (front_zone_width_ + front_side_zone_width_); 1066 | right_front_side_zone.bx = right_front_side_zone.ax + front_zone_range; 1067 | right_front_side_zone.by = -1.0 * front_zone_width_; 1068 | if( IsInBoundingBox(right_front_side_zone, x, y) ) 1069 | { 1070 | // point is in the box 1071 | sensor_summary_.Increment(RIGHT_FRONT_SIDE_ZONE, sensor_type); 1072 | } 1073 | 1074 | } 1075 | 1076 | void SafetyControl::DisplayCollisionMarker(int zone) 1077 | { 1078 | // Display markers to indicate when robot is at risk of collision (or cliff, etc.) 1079 | // For Markers info, see http://wiki.ros.org/rviz/Tutorials/Markers%3A%20Basic%20Shapes 1080 | 1081 | // ROS_INFO("DBG: DisplayCollisionMarker called"); 1082 | // printf ("DBG DisplayCollisionMarker called for zone %d\n", zone); 1083 | 1084 | visualization_msgs::Marker marker; 1085 | marker.header.frame_id = "base_link"; 1086 | marker.header.stamp = ros::Time::now(); 1087 | 1088 | // Any marker sent with the same namespace and id will overwrite the old one 1089 | marker.ns = "safety_control"; 1090 | marker.id = zone; // We use the zone number to make the id unique 1091 | 1092 | uint32_t shape = visualization_msgs::Marker::CUBE; 1093 | marker.type = shape; 1094 | 1095 | // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 1096 | marker.action = visualization_msgs::Marker::ADD; 1097 | marker.color.r = 1.0f; 1098 | marker.color.g = 0.0f; 1099 | marker.color.b = 1.0f; 1100 | marker.color.a = 1.0; 1101 | marker.pose.orientation.x = 0.0; 1102 | marker.pose.orientation.y = 0.0; 1103 | marker.pose.orientation.z = 0.0; 1104 | marker.pose.orientation.w = 1.0; 1105 | 1106 | marker.pose.position.z = 0.02; // shift up to ground plane 1107 | marker.scale.z = 0.04; // fixed marker height 1108 | marker.lifetime = ros::Duration(1.0); // seconds 1109 | 1110 | switch( zone ) 1111 | { 1112 | case LEFT_FRONT_ZONE: 1113 | marker.scale.x = front_zone_range_; 1114 | marker.scale.y = front_zone_width_; 1115 | marker.pose.position.x = (robot_length_ + front_zone_range_) / 2.0; 1116 | marker.pose.position.y = front_zone_width_ / 2.0; 1117 | break; 1118 | 1119 | case RIGHT_FRONT_ZONE: 1120 | marker.scale.x = front_zone_range_; 1121 | marker.scale.y = front_zone_width_; 1122 | marker.pose.position.x = (robot_length_ + front_zone_range_) / 2.0; 1123 | marker.pose.position.y = -1.0 * front_zone_width_ / 2.0; 1124 | break; 1125 | 1126 | case LEFT_REAR_ZONE: 1127 | marker.scale.x = rear_zone_range_; 1128 | marker.scale.y = rear_zone_width_; 1129 | marker.pose.position.x = -1.0 * (robot_length_ + rear_zone_range_) / 2.0; 1130 | marker.pose.position.y = rear_zone_width_ / 2.0; 1131 | break; 1132 | 1133 | case RIGHT_REAR_ZONE: 1134 | marker.scale.x = rear_zone_range_; 1135 | marker.scale.y = rear_zone_width_; 1136 | marker.pose.position.x = -1.0 * (robot_length_ + rear_zone_range_) / 2.0; 1137 | marker.pose.position.y = -1.0 * rear_zone_width_ / 2.0; 1138 | break; 1139 | 1140 | case LEFT_SIDE_ZONE: 1141 | marker.scale.x = robot_length_ ; 1142 | marker.scale.y = side_zone_range_; 1143 | marker.pose.position.x = 0.0; // sides are aligned with center of robot 1144 | marker.pose.position.y = (robot_width_ + side_zone_range_) / 2.0; 1145 | break; 1146 | 1147 | case RIGHT_SIDE_ZONE: 1148 | marker.scale.x = robot_length_ ; 1149 | marker.scale.y = side_zone_range_; 1150 | marker.pose.position.x = 0.0; // sides are aligned with center of robot 1151 | marker.pose.position.y = -1.0 * (robot_width_ + side_zone_range_) / 2.0; 1152 | break; 1153 | 1154 | 1155 | case LEFT_FRONT_SIDE_ZONE: 1156 | marker.scale.x = front_zone_range_; 1157 | marker.scale.y = front_side_zone_width_; 1158 | marker.pose.position.x = (robot_length_ + front_zone_range_) / 2.0; 1159 | marker.pose.position.y = front_zone_width_ + (front_side_zone_width_ / 2.0); 1160 | 1161 | marker.color.r = 1.0f; 1162 | marker.color.g = 1.0f; 1163 | marker.color.b = 0.0f; 1164 | 1165 | break; 1166 | 1167 | case RIGHT_FRONT_SIDE_ZONE: 1168 | marker.scale.x = front_zone_range_; 1169 | marker.scale.y = front_side_zone_width_; 1170 | marker.pose.position.x = (robot_length_ + front_zone_range_) / 2.0; 1171 | marker.pose.position.y = -1.0 * (front_zone_width_ + (front_side_zone_width_ / 2.0)); 1172 | 1173 | marker.color.r = 1.0f; 1174 | marker.color.g = 1.0f; 1175 | marker.color.b = 0.0f; 1176 | 1177 | break; 1178 | 1179 | 1180 | default: 1181 | ROS_ERROR("SafetyControl: Bad Zone!"); 1182 | return; // don't publish anyting 1183 | break; 1184 | } 1185 | //ROS_INFO("DBG: Publishing Marker"); 1186 | marker_pub_.publish(marker); 1187 | 1188 | } 1189 | 1190 | 1191 | 1192 | /////////////////////////////////////////////////////////////////////////////////////////////////////////// 1193 | int main(int argc, char** argv) 1194 | { 1195 | ROS_INFO("SafetyControl: Initializing... "); 1196 | ros::init(argc, argv, "safety_control"); 1197 | 1198 | ros::NodeHandle n; 1199 | //ros::NodeHandle n("~"); 1200 | SafetyControl safety_control(n); 1201 | ros::spin(); 1202 | return 0; 1203 | 1204 | } 1205 | 1206 | 1207 | --------------------------------------------------------------------------------