$search
00001 /* 00002 * Navigator passing controller 00003 * 00004 * Copyright (C) 2007, 2010, Austin Robot Technology 00005 * 00006 * License: Modified BSD Software License Agreement 00007 * 00008 * $Id: passing.cc 1876 2011-11-26 23:48:08Z jack.oquin $ 00009 */ 00010 00011 #include "navigator_internal.h" 00012 #include "Controller.h" 00013 #include "course.h" 00014 #include "obstacle.h" 00015 #include "passing.h" 00016 00017 #include "halt.h" 00018 #include "follow_safely.h" 00019 #include "slow_for_curves.h" 00020 00021 #include <art/DARPA_rules.h> 00022 00023 Passing::Passing(Navigator *navptr, int _verbose): 00024 Controller(navptr, _verbose) 00025 { 00026 //avoid = new Avoid(navptr, _verbose); 00027 halt = new Halt(navptr, _verbose); 00028 follow_safely = new FollowSafely(navptr, _verbose); 00029 slow_for_curves = new SlowForCurves(navptr, _verbose); 00030 reset_me(); 00031 }; 00032 00033 Passing::~Passing() 00034 { 00035 //delete avoid; 00036 delete halt; 00037 delete follow_safely; 00038 delete slow_for_curves; 00039 }; 00040 00041 // follow passing lane 00042 // 00043 // result: 00044 // OK if able to continue or waiting for passing lane to clear; 00045 // Blocked, if lane blocked; 00046 // Finished, if original lane reached. 00047 Controller::result_t Passing::control(pilot_command_t &pcmd) 00048 { 00049 #if 0 // not doing avoid right now 00050 pilot_command_t incmd = pcmd; // copy of original input 00051 #endif // not doing avoid right now 00052 00053 if (!in_passing_lane 00054 && course->in_lane(MapPose(estimate->pose.pose))) 00055 { 00056 // reached passing lane 00057 in_passing_lane = true; 00058 } 00059 00060 if (!in_passing_lane // not in passing lane yet? 00061 && !obstacle->passing_lane_clear()) // clear to go? 00062 { 00063 trace("passing (not clear) controller", pcmd); 00064 return halt->control(pcmd); 00065 } 00066 00067 ROS_DEBUG("Go to waypoint %s in passing lane", 00068 ElementID(order->waypt[1].id).name().str); 00069 00070 // reduce speed while passing 00071 nav->reduce_speed_with_min(pcmd, config_->passing_speed); 00072 00073 // adjust speed for any upcoming curves 00074 result_t result = slow_for_curves->control(pcmd); 00075 00076 if (done_passing()) 00077 { 00078 ROS_WARN("passing completed"); 00079 result = Finished; 00080 } 00081 00082 // set heading to desired course 00083 course->desired_heading(pcmd); 00084 00085 // check if way-point reached 00086 course->lane_waypoint_reached(); 00087 00088 #if 0 // not doing avoid right now 00089 result_t avoid_result = avoid->control(pcmd, incmd); 00090 if (result == OK) 00091 { 00092 result = avoid_result; 00093 } 00094 #endif // not doing avoid right now 00095 00096 trace("passing controller", pcmd, result); 00097 00098 return result; 00099 }; 00100 00101 // return true when passing completed 00102 bool Passing::done_passing(void) 00103 { 00104 bool done = false; 00105 double dist_since_passing = 00106 course->distance_in_plan(course->start_pass_location, 00107 MapPose(estimate->pose.pose)); 00108 if (dist_since_passing > config_->passing_distance) 00109 { 00110 // TODO: compute aim point for reentering lane? 00111 00112 // see if clear to return to passed lane 00113 float ahead, behind; 00114 obstacle->closest_in_lane(course->passed_lane, ahead, behind); 00115 if (ahead >= config_->passing_clearance_ahead 00116 && behind >= config_->passing_clearance_behind) 00117 { 00118 done = true; 00119 } 00120 } 00121 return done; 00122 } 00123 00124 // reset all subordinate controllers 00125 void Passing::reset(void) 00126 { 00127 trace_reset("Passing"); 00128 reset_me(); 00129 //avoid->reset(); 00130 halt->reset(); 00131 follow_safely->reset(); 00132 slow_for_curves->reset(); 00133 } 00134 00135 // reset this controller only 00136 void Passing::reset_me(void) 00137 { 00138 navdata->reverse = false; 00139 in_passing_lane = false; 00140 }