adjacent_right.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_right.h>
00019 
00020 namespace observers
00021 {
00022 
00023 AdjacentRight::AdjacentRight(art_observers::ObserversConfig &config):
00024   Observer(config,
00025            art_msgs::Observation::Adjacent_right,
00026            std::string("Adjacent Right"))
00027 {
00028   distance_filter_.configure();
00029   velocity_filter_.configure();
00030 }
00031 
00032 AdjacentRight::~AdjacentRight()
00033 {
00034 }
00035 
00041 art_msgs::Observation
00042   AdjacentRight::update(const art_msgs::ArtLanes &local_map,
00043                          const art_msgs::ArtLanes &obstacles,
00044                          MapPose pose_) {
00045 
00046   art_msgs::ArtLanes adj_lane_quads = quad_ops::filterAdjacentLanes
00047                                         (pose_, local_map, -1);
00048 
00049   art_msgs::ArtLanes adj_lane_obstacles = getObstaclesInLane(obstacles, adj_lane_quads);
00050   
00051   //Finding closest poly in left lane
00052   PolyOps polyOps_right;
00053   std::vector<poly> adj_polys_right;
00054   int index_adj = -1;
00055   polyOps_right.GetPolys(adj_lane_quads, adj_polys_right);
00056   index_adj = polyOps_right.getClosestPoly(adj_polys_right, pose_.map.x, pose_.map.y);
00057  
00058   float distance = std::numeric_limits<float>::infinity();
00059   if (adj_lane_obstacles.polygons.size()!=0)
00060     {
00061       // Get distance along road from robot to nearest obstacle
00062       int target_id = adj_lane_obstacles.polygons[0].poly_id;
00063       distance = 0;
00064       // Check to see what direction the right lane is going in
00065       if(adj_lane_quads.polygons[index_adj].poly_id < adj_lane_obstacles.polygons[0].poly_id) {
00066         for (size_t i = index_adj; i < adj_lane_quads.polygons.size(); i++)
00067           {
00068             distance += adj_lane_quads.polygons[i].length;
00069             if (adj_lane_quads.polygons[i].poly_id == target_id)
00070               break;
00071           }
00072       } else {
00073         for( size_t i = index_adj; i<adj_lane_quads.polygons.size(); i--) {
00074           distance += adj_lane_quads.polygons[i].length;
00075           if(adj_lane_quads.polygons[i].poly_id == target_id)
00076             break;
00077         }
00078       }
00079     }
00080 
00081   // Filter the distance by averaging over time
00082   float filt_distance;
00083   distance_filter_.update(distance, filt_distance);
00084   
00085   // Calculate velocity of object (including filter)
00086   float prev_distance = observation_.distance;
00087   ros::Time current_update(ros::Time::now());
00088   double time_change = (current_update - prev_update_).toSec();
00089   float velocity = (filt_distance - prev_distance) / (time_change);
00090   float filt_velocity;
00091   velocity_filter_.update(velocity,filt_velocity);
00092   prev_update_ = current_update; // Reset prev_update time
00093 
00094   // Time to intersection (infinite if obstacle moving away)
00095   double time = std::numeric_limits<float>::infinity();
00096   if (filt_velocity < 0)      // Object getting closer, will intersect
00097     {
00098       if (filt_velocity > -0.1)     // avoid dividing by a tiny number
00099         {
00100           filt_velocity = 0.1;
00101         }
00102       time = fabs(filt_distance / filt_velocity);
00103     }
00104 
00105   // return the observation
00106   observation_.distance = filt_distance;
00107   observation_.velocity = filt_velocity;
00108   observation_.time = time;
00109   observation_.clear =  (time > 10.0);
00110   observation_.applicable = (velocity_filter_.isFull());
00111                    
00112   return observation_;
00113 }
00114 }
00115 


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