obstacle.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*-
00002  *
00003  *  Navigator obstacle class
00004  *
00005  *  Copyright (C) 2007, 2010, Austin Robot Technology
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: obstacle.h 1638 2011-08-11 06:41:43Z jack.oquin $
00009  */
00010 
00011 #ifndef _OBSTACLE_HH_
00012 #define _OBSTACLE_HH_
00013 
00014 #include <vector>
00015 #include "ntimer.h"
00016 
00017 #include <art_msgs/ObservationArray.h>
00018 
00024 class Obstacle
00025 {
00026  public:
00027 
00029   Obstacle(Navigator *_nav, int _verbose);
00030 
00032   ~Obstacle()
00033   {
00034     delete blockage_timer;
00035   };
00036 
00041   bool blockage_timeout(void)
00042   {
00043     return blockage_timer->Check();
00044   }
00045 
00047   void blocked(void)
00048   {
00049     if (verbose >= 5)
00050       ART_MSG(3, "Starting blockage timer");
00051     blockage_timer->Start(config_->blockage_timeout_secs);
00052   }
00053 
00055   bool car_approaching();
00056 
00058   void closest_in_lane(const poly_list_t &lane, float &ahead, float &behind);
00059 
00061   bool initialized(void)
00062   {
00063     //return lasers->have_ranges;
00064     return true;
00065   };
00066 
00068   float maximum_range(void) {return max_range;}
00069 
00071   art_msgs::Observation observation(art_msgs::Observation::_oid_type oid)
00072   {
00073     return obstate.obs[oid];
00074   }
00075 
00077   bool observer_clear(art_msgs::Observation::_oid_type oid)
00078   {
00079     bool clear = obstate.obs[oid].clear && obstate.obs[oid].applicable;
00080 
00081     // if waiting on observers, reset the blockage_timer
00082     if (!clear)
00083       blocked();
00084     return clear;
00085   }
00086 
00087   void observers_message(const art_msgs::ObservationArrayConstPtr obs_msg);
00088 
00090   bool passing_lane_clear(void);
00091 
00093   void reset(void);
00094 
00096   void unblocked(void)
00097   {
00098     if (verbose >= 5)
00099       ART_MSG(3, "Canceling blockage timer");
00100     blockage_timer->Cancel();
00101   }
00102   
00104   void update_blockage_state(void)
00105   {
00106     if (navdata->stopped != was_stopped ||
00107         !navdata->stopped)
00108       {
00109         if (navdata->stopped)
00110           blocked();
00111         else
00112           unblocked();
00113         if (verbose >= 4 &&
00114             navdata->stopped != was_stopped)
00115           ART_MSG(8, "vehicle %s moving",
00116                   (navdata->stopped? "stopped": "started"));
00117         was_stopped = navdata->stopped;
00118       }
00119   }
00120 
00121  private:
00122 
00123   // parameters
00124   float max_range;                      //< maximum scan range
00125 
00126   ros::Subscriber obs_sub_;             //< observations subscription
00127 
00128   // observers data
00129   art_msgs::ObservationArray obstate;   //< current observers state
00130 
00131   // blockage timer
00132   NavTimer *blockage_timer;
00133   bool was_stopped;                     // previous cycle's stop state
00134 
00135   // constructor parameters
00136   int verbose;                          // message verbosity level
00137   Navigator *nav;                       // internal navigator class
00138 
00139   // convenience pointers to Navigator class data
00140   PolyOps* pops;                        // polygon operations class
00141   Course* course;                       // course planning class
00142   art_msgs::Order *order;               // current commander order
00143   art_msgs::NavigatorState *navdata;    // current navigator state
00144   nav_msgs::Odometry *odom;             // current odometry position
00145   nav_msgs::Odometry *estimate;         // estimated control position
00146   const Config *config_;                // current configuration
00147 
00148   // returns true if obstacle is within the specified lane
00149   bool in_lane(MapXY location, const poly_list_t &lane, int start_index);
00150 
00151 #if 0 // ignoring obstacles at the moment
00152   // conversions between laser scan indices and bearings
00153   float laser_scan_bearing(unsigned index, const player_laser_data_t &scan)
00154   {
00155     return scan.min_angle + index * scan.resolution;
00156   }
00157   unsigned laser_scan_index(float bearing, const player_laser_data_t &scan)
00158   {
00159     unsigned index =
00160       (unsigned) rintf((bearing - scan.min_angle) / scan.resolution);
00161     if (index < 0)
00162       index = 0;
00163     else if (index > scan.ranges_count)
00164       index = scan.ranges_count;
00165     return index;
00166   }
00167 #endif // ignoring obstacles at the moment
00168 };
00169 
00170 #endif // _OBSTACLE_HH_
 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