nearest_backward.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/nearest_backward.h>
00019 
00020 namespace observers
00021 {
00022 
00023 NearestBackward::NearestBackward(art_observers::ObserversConfig &config):
00024   Observer(config,
00025            art_msgs::Observation::Nearest_backward,
00026            std::string("Nearest_backward"))
00027 {
00028   distance_filter_.configure();
00029   velocity_filter_.configure();
00030 }
00031 
00032 NearestBackward::~NearestBackward() 
00033 {
00034 }
00035 
00036 // \brief  Update message with new data.
00037 art_msgs::Observation
00038   NearestBackward::update(const art_msgs::ArtLanes &local_map,
00039                           const art_msgs::ArtLanes &obstacles, 
00040                           MapPose pose_)
00041 {
00042 
00043   PolyOps polyOps;
00044   std::vector<poly> map_polys;
00045   int poly_index = -1;
00046   polyOps.GetPolys(local_map, map_polys);
00047   poly_index = polyOps.getClosestPoly(map_polys, pose_.map.x, pose_.map.y);
00048 
00049   art_msgs::ArtQuadrilateral robot_quad1 = local_map.polygons[poly_index];
00050 
00051   // get quadrilaterals ahead in the current lane
00052   art_msgs::ArtLanes lane_quads =
00053     quad_ops::filterLanes(robot_quad1, local_map,
00054                           *quad_ops::compare_backward_seg_lane);
00055 
00056   // get obstacles ahead in the current lane
00057   art_msgs::ArtLanes lane_obstacles =
00058     quad_ops::filterLanes(robot_quad1, obstacles,
00059                           *quad_ops::compare_backward_seg_lane);
00060 
00061   // reverse the vectors because the code that follows expects
00062   // polygons in order of distance from base polygon
00063   std::reverse(lane_quads.polygons.begin(), lane_quads.polygons.end());
00064   std::reverse(lane_obstacles.polygons.begin(), lane_obstacles.polygons.end());
00065 
00066   float distance = std::numeric_limits<float>::infinity();
00067   if (lane_obstacles.polygons.size()!=0)
00068     {
00069       // get distance along road from robot to nearest obstacle
00070       int target_id = lane_obstacles.polygons[0].poly_id;
00071       distance = 0;
00072       for (size_t i = 0; i < lane_quads.polygons.size(); i++)
00073         {
00074           distance += lane_quads.polygons[i].length;
00075           if (lane_quads.polygons[i].poly_id == target_id)
00076             break;
00077         }
00078     }
00079 
00080   // filter the distance by averaging over time
00081   float filt_distance;
00082   distance_filter_.update(distance, filt_distance);
00083   
00084   // calculate velocity of object (including filter)
00085   float prev_distance = observation_.distance;
00086   ros::Time current_update(ros::Time::now());
00087   double time_change = (current_update - prev_update_).toSec();
00088   float velocity = (filt_distance - prev_distance) / (time_change);
00089   float filt_velocity;
00090   velocity_filter_.update(velocity,filt_velocity);
00091   prev_update_ = current_update; // Reset prev_update time
00092 
00093   // Time to intersection (infinite if obstacle moving away)
00094   double time = std::numeric_limits<float>::infinity();
00095   if (filt_velocity < 0)      // Object getting closer, will intersect
00096     {
00097       if (filt_velocity > -0.1)     // avoid dividing by a tiny number
00098         {
00099           filt_velocity = 0.1;
00100         }
00101       time = fabs(filt_distance / filt_velocity);
00102     }
00103 
00104   // return the observation
00105   observation_.distance = filt_distance;
00106   observation_.velocity = filt_velocity;
00107   observation_.time = time;
00108   observation_.clear =  (time > 10.0);
00109   observation_.applicable = (velocity_filter_.isFull());
00110                    
00111   return observation_;
00112 }
00113 
00114 }; // namespace observers


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