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 };
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:42:11