adjacent_left.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2010 UT-Austin & Austin Robot Technology, Michael Quinlan
00003  *  Copyright (C) 2011 UT-Austin & Austin Robot Technology
00004  *  License: Modified BSD Software License 
00005  */
00006 
00017 #include <art_observers/QuadrilateralOps.h>
00018 #include <art_observers/adjacent_left.h>
00019 
00020 namespace observers
00021 {
00022 
00023 AdjacentLeft::AdjacentLeft(art_observers::ObserversConfig &config):
00024   Observer(config,
00025            art_msgs::Observation::Adjacent_left,
00026            std::string("Adjacent Left"))
00027 {
00028   distance_filter_.configure();
00029   velocity_filter_.configure();
00030 }
00031 
00032 AdjacentLeft::~AdjacentLeft()
00033 {
00034 }
00035 
00041 art_msgs::Observation
00042   AdjacentLeft::update(const art_msgs::ArtLanes &local_map,
00043                          const art_msgs::ArtLanes &obstacles,
00044                          MapPose pose_)
00045 {
00046   
00047   art_msgs::ArtLanes adj_lane_quads = quad_ops::filterAdjacentLanes
00048                                         (pose_, local_map, 1);
00049 
00050   art_msgs::ArtLanes adj_lane_obstacles = getObstaclesInLane(obstacles, adj_lane_quads);
00051   
00052   //Finding closest poly in left lane
00053   PolyOps polyOps_left;
00054   std::vector<poly> adj_polys_left;
00055   int index_adj = -1;
00056   polyOps_left.GetPolys(adj_lane_quads, adj_polys_left);
00057   index_adj = polyOps_left.getClosestPoly(adj_polys_left, pose_.map.x, pose_.map.y);
00058  
00059   float distance = std::numeric_limits<float>::infinity();
00060   if (adj_lane_obstacles.polygons.size()!=0)
00061     {
00062       // Get distance along road from robot to nearest obstacle
00063       int target_id = adj_lane_obstacles.polygons[0].poly_id;
00064       distance = 0;
00065       // Check to see direction of the adjacent lane
00066       if(adj_lane_quads.polygons[index_adj].poly_id < adj_lane_obstacles.polygons[0].poly_id) {
00067         for (size_t i = index_adj; i < adj_lane_quads.polygons.size(); i++)
00068           {
00069             distance += adj_lane_quads.polygons[i].length;
00070             if (adj_lane_quads.polygons[i].poly_id == target_id)
00071               break;
00072           }
00073       } else {
00074         for( size_t i = index_adj; i<adj_lane_quads.polygons.size(); i--) {
00075           distance += adj_lane_quads.polygons[i].length;
00076           if(adj_lane_quads.polygons[i].poly_id == target_id)
00077             break;
00078         }
00079       }
00080     }
00081 
00082   // Filter the distance by averaging over time
00083   float filt_distance;
00084   distance_filter_.update(distance, filt_distance);
00085   
00086   // Calculate velocity of object (including filter)
00087   float prev_distance = observation_.distance;
00088   ros::Time current_update(ros::Time::now());
00089   double time_change = (current_update - prev_update_).toSec();
00090   float velocity = (filt_distance - prev_distance) / (time_change);
00091   float filt_velocity;
00092   velocity_filter_.update(velocity,filt_velocity);
00093   prev_update_ = current_update; // Reset prev_update time
00094 
00095   // Time to intersection (infinite if obstacle moving away)
00096   double time = std::numeric_limits<float>::infinity();
00097 
00098   if (filt_velocity < 0)
00099     {
00100       // Object getting closer
00101       if (filt_velocity > -0.1)
00102         {
00103           // avoid dividing by a tiny number
00104           filt_velocity = 0.1;
00105         }
00106       time = fabs(filt_distance / filt_velocity);
00107     }
00108 
00109   if (filt_velocity < 0)
00110     {
00111       // Object getting closer
00112       if (filt_velocity > -0.1)
00113         {
00114           // avoid dividing by a tiny number
00115           filt_velocity = 0.1;
00116         }
00117       time = fabs(filt_distance / filt_velocity);
00118     }
00119 
00120   // return the observation
00121   observation_.distance = filt_distance;
00122   observation_.velocity = filt_velocity;
00123   observation_.time = time;
00124   observation_.clear =  (time > 10.0);
00125   observation_.applicable = (velocity_filter_.isFull());
00126                    
00127   return observation_;
00128 }
00129 }; // namespace observers


art_observers
Author(s): Michael Quinlan, Jack O'Quin, Corbyn Salisbury
autogenerated on Fri Jan 3 2014 11:09:22