obstacle.cc
Go to the documentation of this file.
00001 /*
00002  *  Navigator obstacle class
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id: obstacle.cc 1638 2011-08-11 06:41:43Z jack.oquin $
00008  */
00009 
00010 //#include <art/DARPA_rules.h>
00011 #include <art/infinity.h>
00012 //#include <art/Safety.h>
00013 
00014 #include "navigator_internal.h"
00015 #include "course.h"
00016 #include "obstacle.h"
00017 
00018 // Provide shorter name for observation message.
00019 using art_msgs::Observation;
00020 
00021 // Constructor
00022 Obstacle::Obstacle(Navigator *_nav, int _verbose)
00023 {
00024   verbose = _verbose;
00025   nav = _nav;
00026 
00027   // copy convenience pointers to Navigator class data
00028   course = nav->course;
00029   estimate = &nav->estimate;
00030   navdata = &nav->navdata;
00031   odom = nav->odometry;
00032   order = &nav->order;
00033   pops = nav->pops;
00034   config_ = &nav->config_;
00035 
00036   // TODO Make this a parameter
00037   max_range = 80.0;
00038 
00039   // initialize observers state to all clear in case that driver is
00040   // not subscribed or not publishing data
00041   obstate.obs.resize(Observation::N_Observers);
00042   for (unsigned i = 0; i < Observation::N_Observers; ++i)
00043     {
00044       obstate.obs[i].oid = i;
00045       obstate.obs[i].clear = true;
00046       obstate.obs[i].applicable = true;
00047     }
00048 
00049   // exception: initialize Nearest_forward not applicable
00050   obstate.obs[Observation::Nearest_forward].applicable = false;
00051 
00052   // allocate timers
00053   blockage_timer = new NavTimer();
00054   was_stopped = false;
00055 
00056   reset();
00057 
00058   ros::NodeHandle node;
00059   obs_sub_ = node.subscribe("observations", 10,
00060                             &Obstacle::observers_message, this,
00061                             ros::TransportHints().tcpNoDelay(true));
00062 }
00063 
00064 // is there a car approaching from ahead in our lane?
00065 //
00066 //  Note: must *not* reset blockage_timer.
00067 //
00068 bool Obstacle::car_approaching()
00069 {
00070   if (config_->offensive_driving)
00071     return false;
00072 
00073   Observation::_oid_type oid = Observation::Nearest_forward;
00074   if (obstate.obs[oid].clear || !obstate.obs[oid].applicable)
00075     {
00076       if (verbose >= 4)
00077         ART_MSG(6, "no known car approaching from ahead");
00078       return false;                     // clear ahead
00079     }
00080 
00081   // estimate absolute speed of obstacle from closing velocity and
00082   // vehicle speed
00083   float rel_speed = obstate.obs[oid].velocity;
00084 #if 0
00085   // use vehicle speed at time of observation
00086   float abs_speed = rel_speed - obstate.odom.twist.twist.linear.x;
00087 #else
00088   // use current vehicle speed
00089   float abs_speed = rel_speed - odom->twist.twist.linear.x;
00090 #endif
00091 
00092   if (verbose >= 3)
00093     ART_MSG(6, "obstacle observed closing at %.3fm/s, absolute speed %.3f",
00094             rel_speed, abs_speed);
00095 
00096   return (abs_speed > config_->min_approach_speed);
00097 }
00098 
00099 // Return distances of closest obstacles ahead and behind in a lane.
00100 //
00101 // entry:
00102 //      lane = list of polygons to check
00103 // exit:
00104 //      ahead = distance along lane to closest obstacle ahead
00105 //      behind = distance along lane to closest obstacle behind
00106 //      (returns Infinite::distance if none in range)
00107 //
00108 void Obstacle::closest_in_lane(const poly_list_t &lane,
00109                                float &ahead, float &behind)
00110 {
00111   // values to return if no obstacles found
00112   ahead = Infinite::distance;
00113   behind = Infinite::distance;
00114 
00115   int closest_poly_index =
00116     pops->getClosestPoly(lane, MapPose(estimate->pose.pose));
00117   if (lane.empty() || closest_poly_index < 0)
00118     {
00119       ROS_DEBUG_STREAM("no closest polygon in lane (size "
00120                        << lane.size() << ")");
00121       return;
00122     }
00123 
00124 #if 0 // ignoring obstacles at the moment
00125   
00126   for (uint i=0; i< lasers->all_obstacle_list.size(); i++)
00127     if (in_lane(lasers->all_obstacle_list.at(i), lane, 0))
00128       {
00129         float distance = pops->distanceAlongLane(lane, estimate->pos,
00130                                                  lasers->all_obstacle_list.at(i));                                      
00131         if (distance < 0)
00132           behind = fminf(behind, -distance);
00133         else
00134           ahead = fminf(ahead, distance);
00135       }
00136 
00137 #endif // ignoring obstacles at the moment
00138 
00139   ROS_DEBUG("Closest obstacles in lane %s are %.3fm ahead and %.3fm behind",
00140             lane.at(closest_poly_index).start_way.lane_name().str,
00141             ahead, behind);
00142 }
00143 
00144 // returns true if obstacle is within the specified lane
00145 bool Obstacle::in_lane(MapXY location, const poly_list_t &lane,
00146                        int start_index)
00147 {
00148   start_index=std::max(0,start_index);
00149   start_index=std::min(start_index, int(lane.size()-1));
00150 
00151   poly in_poly= lane.at(start_index);
00152 
00153   // figure out what polygon contains the obstacle
00154   for (unsigned j = start_index; j < lane.size(); j++)
00155     {
00156       poly curPoly = lane[j];
00157       if (pops->pointInPoly_ratio(location, curPoly, config_->lane_width_ratio))
00158         {
00159           // this obstacle is within that lane
00160           if (verbose >= 5)
00161             {
00162               ART_MSG(8, "obstacle at (%.3f, %.3f) is in lane %s, polygon %d",
00163                       location.x, location.y, 
00164                       in_poly.start_way.lane_name().str, curPoly.poly_id);
00165             }
00166           return true;                  // terminate polygon search
00167         }
00168     }
00169   
00170   if (verbose >= 6)
00171     ART_MSG(8, "obstacle at (%.3f, %.3f) is outside our lane",
00172             location.x, location.y);
00173 
00174   return false;
00175 }
00176 
00181 void
00182   Obstacle::observers_message(const art_msgs::ObservationArrayConstPtr obs_msg)
00183 {
00184   // Messages could arrive out of order, only track the most recent
00185   // time stamp.
00186   obstate.header.stamp = std::max(obstate.header.stamp,
00187                                   obs_msg->header.stamp);
00188 
00189   // obstate contains all the latest observations received, update
00190   // only the ones present in this message.
00191   for (uint32_t i = 0; i < obs_msg->obs.size(); ++i)
00192     {
00193       obstate.obs[obs_msg->obs[i].oid] = obs_msg->obs[i];
00194       // TODO (maybe) track time stamps separately for each OID?
00195     }
00196 }
00197 
00198 // return true when observer reports passing lane clear
00199 bool Obstacle::passing_lane_clear(void)
00200 {
00201   if (course->passing_left)
00202     return observer_clear(Observation::Adjacent_left);
00203   else
00204     return observer_clear(Observation::Adjacent_right);
00205 }
00206 
00207 // reset obstacle class
00208 void Obstacle::reset(void)
00209 {
00210   unblocked();
00211 }
 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