├── CMakeLists.txt ├── LICENSE ├── README.md ├── package.xml └── src ├── laser_obstacle_avoidance.cpp └── laser_obstacle_avoidance_zones.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(laser_obstacle_avoidance) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | ) 10 | 11 | ## System dependencies are found with CMake's conventions 12 | # find_package(Boost REQUIRED COMPONENTS system) 13 | 14 | 15 | ## Uncomment this if the package has a setup.py. This macro ensures 16 | ## modules and global scripts declared therein get installed 17 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 18 | # catkin_python_setup() 19 | 20 | ################################################ 21 | ## Declare ROS messages, services and actions ## 22 | ################################################ 23 | 24 | ## To declare and build messages, services or actions from within this 25 | ## package, follow these steps: 26 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 27 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 28 | ## * In the file package.xml: 29 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 30 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 31 | ## pulled in transitively but can be declared for certainty nonetheless: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a run_depend tag for "message_runtime" 34 | ## * In this file (CMakeLists.txt): 35 | ## * add "message_generation" and every package in MSG_DEP_SET to 36 | ## find_package(catkin REQUIRED COMPONENTS ...) 37 | ## * add "message_runtime" and every package in MSG_DEP_SET to 38 | ## catkin_package(CATKIN_DEPENDS ...) 39 | ## * uncomment the add_*_files sections below as needed 40 | ## and list every .msg/.srv/.action file to be processed 41 | ## * uncomment the generate_messages entry below 42 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 43 | 44 | ## Generate messages in the 'msg' folder 45 | # add_message_files( 46 | # FILES 47 | # Message1.msg 48 | # Message2.msg 49 | # ) 50 | 51 | ## Generate services in the 'srv' folder 52 | # add_service_files( 53 | # FILES 54 | # Service1.srv 55 | # Service2.srv 56 | # ) 57 | 58 | ## Generate actions in the 'action' folder 59 | # add_action_files( 60 | # FILES 61 | # Action1.action 62 | # Action2.action 63 | # ) 64 | 65 | ## Generate added messages and services with any dependencies listed here 66 | # generate_messages( 67 | # DEPENDENCIES 68 | # std_msgs # Or other packages containing msgs 69 | # ) 70 | 71 | ################################### 72 | ## catkin specific configuration ## 73 | ################################### 74 | ## The catkin_package macro generates cmake config files for your package 75 | ## Declare things to be passed to dependent projects 76 | ## INCLUDE_DIRS: uncomment this if you package contains header files 77 | ## LIBRARIES: libraries you create in this project that dependent projects also need 78 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 79 | ## DEPENDS: system dependencies of this project that dependent projects also need 80 | catkin_package( 81 | # INCLUDE_DIRS include 82 | # LIBRARIES laser_obstacle_avoidance 83 | # CATKIN_DEPENDS roscpp 84 | # DEPENDS system_lib 85 | ) 86 | 87 | ########### 88 | ## Build ## 89 | ########### 90 | 91 | ## Specify additional locations of header files 92 | ## Your package locations should be listed before other locations 93 | # include_directories(include) 94 | include_directories( 95 | ${catkin_INCLUDE_DIRS} 96 | ) 97 | 98 | ## Declare a cpp library 99 | # add_library(laser_obstacle_avoidance 100 | # src/${PROJECT_NAME}/laser_obstacle_avoidance.cpp 101 | # ) 102 | 103 | ## Declare a cpp executable 104 | add_executable(laser_obstacle_avoidance src/laser_obstacle_avoidance.cpp) 105 | add_executable(laser_obstacle_avoidance_zones src/laser_obstacle_avoidance_zones.cpp) 106 | 107 | ## Add cmake target dependencies of the executable/library 108 | ## as an example, message headers may need to be generated before nodes 109 | # add_dependencies(laser_obstacle_avoidance_node laser_obstacle_avoidance_generate_messages_cpp) 110 | 111 | ## Specify libraries to link a library or executable target against 112 | target_link_libraries(laser_obstacle_avoidance 113 | ${catkin_LIBRARIES} 114 | ) 115 | 116 | target_link_libraries(laser_obstacle_avoidance_zones 117 | ${catkin_LIBRARIES} 118 | ) 119 | 120 | ############# 121 | ## Install ## 122 | ############# 123 | 124 | # all install targets should use catkin DESTINATION variables 125 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 126 | 127 | ## Mark executable scripts (Python etc.) for installation 128 | ## in contrast to setup.py, you can choose the destination 129 | # install(PROGRAMS 130 | # scripts/my_python_script 131 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 132 | # ) 133 | 134 | ## Mark executables and/or libraries for installation 135 | # install(TARGETS laser_obstacle_avoidance laser_obstacle_avoidance_node 136 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 137 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 138 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 139 | # ) 140 | 141 | ## Mark cpp header files for installation 142 | # install(DIRECTORY include/${PROJECT_NAME}/ 143 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 144 | # FILES_MATCHING PATTERN "*.h" 145 | # PATTERN ".svn" EXCLUDE 146 | # ) 147 | 148 | ## Mark other files for installation (e.g. launch and bag files, etc.) 149 | # install(FILES 150 | # # myfile1 151 | # # myfile2 152 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 153 | # ) 154 | 155 | ############# 156 | ## Testing ## 157 | ############# 158 | 159 | ## Add gtest based cpp test target and link libraries 160 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_laser_obstacle_avoidance.cpp) 161 | # if(TARGET ${PROJECT_NAME}-test) 162 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 163 | # endif() 164 | 165 | ## Add folders to be run by python nosetests 166 | # catkin_add_nosetests(test) 167 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, JorgeArino 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 18 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 20 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 21 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 22 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 23 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | demo_obstacle_avoidance 2 | ======================= 3 | 4 | A simple obstacle avoidance demo using a LIDAR sensor 5 | 6 | 7 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | laser_obstacle_avoidance 4 | 0.0.0 5 | The laser_obstacle_avoidance package 6 | 7 | 8 | 9 | 10 | gimli 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | roscpp 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /src/laser_obstacle_avoidance.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2005, Brad Kratochvil, Toby Collett, Brian Gerkey, Andrew Howard, ... 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, 9 | this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | * Neither the name of the Player Project nor the names of its contributors 14 | may be used to endorse or promote products derived from this software 15 | without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 21 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 24 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | /* 30 | * laserobstacleavoid.cc 31 | * 32 | * a simple obstacle avoidance demo 33 | * 34 | * @todo: this has been ported to libplayerc++, but not tested 35 | */ 36 | 37 | #include "ros/ros.h" 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #define RAYS 1081 45 | #define PI 3.1415 46 | 47 | using namespace std; 48 | 49 | float min_left_dist; 50 | float min_right_dist; 51 | double l,r; 52 | double newspeed = 0; 53 | double newturnrate = 0; 54 | geometry_msgs::Twist cmd_vel; 55 | ros::Publisher cmd_vel_pub; 56 | 57 | float offset_degrees = 45; // degrees to ignore at the sides 58 | float offset_samples = offset_degrees*4; // 4 samples per degree 59 | 60 | //float max_distance = 100.0; 61 | float max_distance = 0.7; 62 | 63 | float force_turning_right_difference = 20.0; 64 | 65 | float max_speed = 0.1; // m/s 66 | float speed_ratio = max_distance*2/max_speed; 67 | 68 | float max_turning_speed_rad = 0.5; // rad/sec 69 | float turning_ratio_rad = (100+max_distance)/max_turning_speed_rad; 70 | 71 | 72 | double limit(double n, double min, double max) 73 | { 74 | if(n < min) 75 | return min; 76 | else if(n > max) 77 | return max; 78 | else return n; 79 | } 80 | 81 | // degrees to radians 82 | float dtor(double degrees) 83 | { 84 | return degrees * PI / 180; 85 | } 86 | 87 | float get_min_distance_left(sensor_msgs::LaserScan laser) 88 | { 89 | float min_left_dist = 1000.0; 90 | 91 | for(int i=laser.ranges.size()/2; i 0.05 && laser.ranges[i] < 1.0) 119 | { 120 | double x = laser.ranges[i] * sin(laser.angle_min + i * laser.angle_increment); 121 | double y = laser.ranges[i] * cos(laser.angle_min + i * laser.angle_increment); 122 | //cout << "x: " << x << " ,y: " << y << " ,dist: " << laser.ranges[i] << endl; 123 | if ((fabs(x) < x_region) && (y < y_region)) 124 | { 125 | return true; 126 | } 127 | } 128 | } 129 | return false; 130 | } 131 | // TODO: doesn´t work 132 | bool check_front_obstacle_semicircle(sensor_msgs::LaserScan laser, float radius) 133 | { 134 | int offset = 90 * 4; // 4 samples per degree 135 | float x_center = 0.0; 136 | float y_center = 0.5; 137 | float dist_hokuyo_to_center = 0.25; 138 | 139 | for(int i=offset; i 0.05 && laser.ranges[i] < 1.0) 143 | { 144 | double x = laser.ranges[i] * sin(laser.angle_min + i * laser.angle_increment); 145 | double y = laser.ranges[i] * cos(laser.angle_min + i * laser.angle_increment); 146 | 147 | y = y - dist_hokuyo_to_center; 148 | cout << "x: " << x << " ,y: " << y << " ,dist: " << laser.ranges[i] << endl; 149 | return true; 150 | float dist = sqrt(x*x + y*y); 151 | if(dist < radius) 152 | { 153 | cout << "distance: " << dist << endl; 154 | return true; 155 | } 156 | 157 | } 158 | } 159 | return false; 160 | } 161 | 162 | void autonomous_behave(const sensor_msgs::LaserScan &laser) 163 | { 164 | newspeed = newturnrate = 0.0; 165 | 166 | min_left_dist = get_min_distance_left(laser); 167 | min_right_dist = get_min_distance_right(laser); 168 | 169 | cout << "----------------------------" << endl; 170 | 171 | 172 | 173 | cout << "Minimum distance to the left: " << min_left_dist << endl; 174 | cout << "Minimum distance to the right: " << min_right_dist << endl; 175 | 176 | 177 | //l = (1e5*min_right_dist)/500-max_distance; 178 | //r = (1e5*min_left_dist)/500-max_distance; 179 | 180 | l = min_left_dist; 181 | r = min_right_dist; 182 | 183 | //cout << "l: " << l << endl; 184 | //cout << "r: " << r << endl; 185 | 186 | //if(abs(l - r) < 1000) 187 | // cout << "Same distance" << endl; 188 | 189 | // limit max distance 190 | if (l > max_distance) 191 | l = max_distance; 192 | if (r > max_distance) 193 | r = max_distance; 194 | 195 | 196 | cout << "l (limited): " << l << endl; 197 | cout << "r(limited): " << r << endl; 198 | 199 | /* 200 | if(l 0) 245 | newturnrate = limit_robot_stuck; 246 | else if(newturnrate < 0) 247 | newturnrate = -limit_robot_stuck; 248 | } 249 | 250 | 251 | cout << "turn rate (rad limited): " << newturnrate << endl; 252 | 253 | //cout << "speed: " << newspeed << "turn: " << newturnrate << endl; 254 | 255 | 256 | 257 | cmd_vel.linear.x = newspeed; 258 | cmd_vel.linear.y = 0.0; 259 | cmd_vel.linear.z = 0.0; 260 | cmd_vel.angular.x = 0.0; 261 | cmd_vel.angular.y = 0.0; 262 | cmd_vel.angular.z = newturnrate; 263 | 264 | cmd_vel_pub.publish(cmd_vel); 265 | 266 | 267 | } 268 | 269 | 270 | 271 | int main(int argc, char **argv) 272 | { 273 | ros::init(argc, argv, "laser_obstacle_avoidance"); 274 | ros::NodeHandle nh; 275 | cmd_vel_pub = nh.advertise("/summit_xl_controller/command", 1); 276 | ros::Subscriber laser_sub = nh.subscribe("scan", 10, autonomous_behave); 277 | 278 | ros::spin(); 279 | } 280 | -------------------------------------------------------------------------------- /src/laser_obstacle_avoidance_zones.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2005, Brad Kratochvil, Toby Collett, Brian Gerkey, Andrew Howard, ... 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, 9 | this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | * Neither the name of the Player Project nor the names of its contributors 14 | may be used to endorse or promote products derived from this software 15 | without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 21 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 24 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | /* 30 | * laser_obstacle_avoidance_zones.cpp 31 | * 32 | * a simple obstacle avoidance demo 33 | * 34 | * @todo: implement dinamic factors 35 | */ 36 | 37 | #include "ros/ros.h" 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #define PI 3.1415 45 | #define INF 100000000 46 | #define min(a,b) a max) 78 | return max; 79 | else return n; 80 | } 81 | 82 | // degrees to radians 83 | float dtor(double degrees) 84 | { 85 | return degrees * PI / 180; 86 | } 87 | 88 | float get_min_distance_left(sensor_msgs::LaserScan laser) 89 | { 90 | float min_left_dist = 1000.0; 91 | 92 | for(int i=laser.ranges.size()/2; i max_degree) 118 | { 119 | float aux = max_degree; 120 | max_degree = min_degree; 121 | min_degree = aux; 122 | } 123 | 124 | //cout << "limit_degrees: " << limit_degrees << endl; 125 | 126 | // to start from 0 127 | min_degree += limit_degrees; 128 | max_degree += limit_degrees; 129 | 130 | //cout << "min_degree: " << min_degree << " max_degree: " << max_degree << endl; 131 | 132 | float min_rad = min_degree / 180 * PI; 133 | float max_rad = max_degree / 180 * PI; 134 | 135 | int min_offset = (int)(min_rad / laser.angle_increment); 136 | int max_offset = (int)(max_rad / laser.angle_increment); 137 | //cout << "min_rad: " << min_rad << " max_rad: " << max_rad << " angle_increment: " << laser.angle_increment << endl; 138 | //cout << "laser_size: " << laser.ranges.size() << " min_offset: " << min_offset << " max_offset: " << max_offset << endl; 139 | 140 | float min = INF; 141 | 142 | for(int i=min_offset; i 0.05 && laser.ranges[i] < 1.0) // 1.0 166 | { 167 | angle = laser.angle_min + i * laser.angle_increment; 168 | double x = laser.ranges[i] * sin(angle); 169 | double y = laser.ranges[i] * cos(angle); 170 | //cout << "x: " << x << " ,y: " << y << " ,dist: " << laser.ranges[i] << endl; 171 | //if(laser.ranges[i] < min) 172 | //{ 173 | //min = laser.ranges[i]; 174 | //x_min = x; 175 | //y_min = y; 176 | //angle_min = angle; 177 | //} 178 | if ((fabs(x) < x_region) && (y < y_region)) 179 | { 180 | //cout << "Point in x: " << x_min << " ,y: " << y_min << " ,angle: " << angle << " ,dist: " << min << endl; 181 | return true; 182 | } 183 | } 184 | } 185 | //cout << "Point in x: " << x_min << " ,y: " << y_min << " ,angle: " << angle << " ,dist: " << min << endl; 186 | return false; 187 | } 188 | 189 | // TODO: doesn´t work 190 | bool check_front_obstacle_semicircle(sensor_msgs::LaserScan laser, float radius) 191 | { 192 | int offset = 90 * 4; // 4 samples per degree 193 | float x_center = 0.0; 194 | float y_center = 0.5; 195 | float dist_hokuyo_to_center = 0.25; 196 | 197 | for(int i=offset; i 0.05 && laser.ranges[i] < 1.0) 201 | { 202 | double x = laser.ranges[i] * sin(laser.angle_min + i * laser.angle_increment); 203 | double y = laser.ranges[i] * cos(laser.angle_min + i * laser.angle_increment); 204 | 205 | y = y - dist_hokuyo_to_center; 206 | cout << "x: " << x << " ,y: " << y << " ,dist: " << laser.ranges[i] << endl; 207 | return true; 208 | float dist = sqrt(x*x + y*y); 209 | if(dist < radius) 210 | { 211 | cout << "distance: " << dist << endl; 212 | return true; 213 | } 214 | 215 | } 216 | } 217 | return false; 218 | } 219 | 220 | void autonomous_behave(const sensor_msgs::LaserScan &laser) 221 | { 222 | newspeed = newturnrate = 0.0; 223 | 224 | min_left_dist = get_min_distance_left(laser); 225 | min_right_dist = get_min_distance_right(laser); 226 | 227 | float l1 = get_min_distance_in_range(laser, 0, 30); 228 | float l2 = get_min_distance_in_range(laser, 30, 60); 229 | float l3 = get_min_distance_in_range(laser, 60, 90); 230 | 231 | float r1 = get_min_distance_in_range(laser, -30, 0); 232 | float r2 = get_min_distance_in_range(laser, -60, -30); 233 | float r3 = get_min_distance_in_range(laser, -90, -60); 234 | 235 | cout << "----------------------------" << endl; 236 | 237 | //cout << "l3: " << l3 << " l2: " << l2 << " l1: " << l1 << " r1: " << r1 << " r2: " << r2 << " r3: " << r3 << endl; 238 | 239 | l = min_left_dist; 240 | r = min_right_dist; 241 | 242 | //cout << "l: " << l << endl; 243 | //cout << "r: " << r << endl; 244 | 245 | //if(abs(l - r) < 1000) 246 | // cout << "Same distance" << endl; 247 | 248 | // limit max distance 249 | if (l > max_distance) 250 | l = max_distance; 251 | if (r > max_distance) 252 | r = max_distance; 253 | 254 | l1 = limit(l1,0,max_distance); 255 | l2 = limit(l2,0,max_distance); 256 | l3 = limit(l3,0,max_distance); 257 | r1 = limit(r1,0,max_distance); 258 | r2 = limit(r2,0,max_distance); 259 | r3 = limit(r3,0,max_distance); 260 | 261 | 262 | 263 | 264 | float v1 = l1 + r1; 265 | float v2 = l2 + r2; 266 | float v3 = l3 + r3; 267 | 268 | // assign weights dinamically 269 | float p1 = 0.5; 270 | float p2 = 0.3; 271 | float p3 = 0.2; 272 | float z1 = min(l1,r1); 273 | float z2 = min(l2,r2); 274 | float z3 = min(l3,r3); 275 | float f1,f2,f3 = 0.0; 276 | 277 | //// @todo: horrible implementation, use std::sort instead 278 | //if(z1 < z2 && z1 < z3) 279 | //{ 280 | //f1 = p1; 281 | //if(z2 < z3) 282 | //{ 283 | //f2 = p2; 284 | //f3 = p3; 285 | //} 286 | //else 287 | //{ 288 | //f2 = p3; 289 | //f3 = p2; 290 | //} 291 | //} 292 | //else if(z2 < z1 && z2 < z3) 293 | //{ 294 | //f2 = p1; 295 | //if(z1 < z3) 296 | //{ 297 | //f1 = p2; 298 | //f3 = p3; 299 | //} 300 | //else 301 | //{ 302 | //f1 = p3; 303 | //f3 = p2; 304 | //} 305 | //} 306 | //else 307 | //{ 308 | //f3 = p1; 309 | //if(z1 < z2) 310 | //{ 311 | //f1 = p2; 312 | //f2 = p3; 313 | //} 314 | //else 315 | //{ 316 | //f1 = p3; 317 | //f2 = p2; 318 | //} 319 | //} 320 | f1 = 0.2; 321 | f2 = 0.3; 322 | f3 = 0.5; 323 | 324 | //cout << "l1: " << l1 << " r1: " << r1 << " ,weight: " << f1 << endl; 325 | //cout << "l2: " << l2 << " r2: " << r2 << " ,weight: " << f2 << endl; 326 | //cout << "l3: " << l3 << " r3: " << r3 << " ,weight: " << f3 << endl; 327 | 328 | 329 | if(!check_front_obstacle(laser,0.35,0.55)) 330 | newspeed = (v1*f1 + v2*f2 + v3*f3)/speed_ratio; 331 | else 332 | { 333 | cout << "colision detected!!" << endl; 334 | newspeed = 0.0; 335 | } 336 | newspeed = limit(newspeed, -max_speed, max_speed); 337 | 338 | 339 | float w1 = l1 - r1; 340 | float w2 = l2 - r2; 341 | float w3 = l3 - r3; 342 | 343 | newturnrate = w1*f1 + w2*f2 + w3*f3; 344 | //cout << "turn rate: " << newturnrate << endl; 345 | 346 | // degrees to radians 347 | //newturnrate = dtor(newturnrate); 348 | 349 | //newturnrate = newturnrate/(2.15/max_turning_speed_rad); // 2.15/max_turn_rate_rad 350 | 351 | //cout << "turn rate (rad): " << newturnrate << endl; 352 | 353 | // limit angular speed 354 | newturnrate = limit(newturnrate, -max_turning_speed_rad, max_turning_speed_rad); 355 | 356 | 357 | 358 | float limit_robot_stuck = 0.25; 359 | 360 | // check if robot is stuck 361 | if(newspeed == 0.0 && newturnrate < limit_robot_stuck) 362 | { 363 | if(newturnrate > 0) 364 | newturnrate = limit_robot_stuck; 365 | else if(newturnrate < 0) 366 | newturnrate = -limit_robot_stuck; 367 | } 368 | 369 | 370 | cout << "turn rate (rad limited): " << newturnrate << endl; 371 | 372 | //cout << "speed: " << newspeed << "turn: " << newturnrate << endl; 373 | 374 | 375 | 376 | cmd_vel.linear.x = newspeed; 377 | cmd_vel.linear.y = 0.0; 378 | cmd_vel.linear.z = 0.0; 379 | cmd_vel.angular.x = 0.0; 380 | cmd_vel.angular.y = 0.0; 381 | cmd_vel.angular.z = newturnrate; 382 | 383 | cmd_vel_pub.publish(cmd_vel); 384 | 385 | 386 | } 387 | 388 | 389 | 390 | int main(int argc, char **argv) 391 | { 392 | ros::init(argc, argv, "laser_obstacle_avoidance"); 393 | ros::NodeHandle nh; 394 | cmd_vel_pub = nh.advertise("/summit_xl_controller/command", 1); 395 | ros::Subscriber laser_sub = nh.subscribe("scan", 10, autonomous_behave); 396 | 397 | ros::spin(); 398 | } 399 | --------------------------------------------------------------------------------