stop_area.cc
Go to the documentation of this file.00001 /* 00002 * Navigator stop line safety area controller 00003 * 00004 * Copyright (C) 2007, 2010, Austin Robot Technology 00005 * 00006 * License: Modified BSD Software License Agreement 00007 * 00008 * $Id: stop_area.cc 872 2010-11-28 13:31:07Z jack.oquin $ 00009 */ 00010 00011 #include "navigator_internal.h" 00012 #include "Controller.h" 00013 #include "course.h" 00014 #include "stop_area.h" 00015 00016 #include <art/DARPA_rules.h> 00017 00018 StopArea::StopArea(Navigator *navptr, int _verbose): 00019 Controller(navptr, _verbose) 00020 { 00021 reset_me(); 00022 }; 00023 00024 StopArea::~StopArea() {}; 00025 00026 // Slow down if stop line safety area reached. 00027 // 00028 // exit: 00029 // course->stop_waypt set 00030 // result: 00031 // OK if within stop line safety area; 00032 // NotApplicable, otherwise. 00033 // 00034 Controller::result_t StopArea::control(pilot_command_t &pcmd) 00035 { 00036 using art_msgs::ArtVehicle; 00037 float wayptdist = (course->stop_waypt_distance(true) 00038 - ArtVehicle::front_bumper_px); 00039 00040 if (wayptdist >= DARPA_rules::stop_line_safety_area) 00041 return NotApplicable; 00042 00043 // reduce speed within stop line safety area 00044 nav->reduce_speed_with_min(pcmd, config_->stop_approach_speed); 00045 00046 if (!in_safety_area) 00047 { 00048 in_safety_area = true; 00049 if (verbose) 00050 ART_MSG(2, "entering stop line safety area, distance %.3fm", 00051 wayptdist); 00052 } 00053 00054 trace("stop_area controller", pcmd, OK); 00055 return OK; // in stop line safety area 00056 }; 00057 00058 // reset controller and all subordinates 00059 void StopArea::reset(void) 00060 { 00061 trace_reset("StopArea"); 00062 reset_me(); 00063 }; 00064 00065 // reset controller state 00066 void StopArea::reset_me(void) 00067 { 00068 in_safety_area = false; 00069 };