nearest_forward.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_forward.h>
00019 #include <art_map/PolyOps.h>
00020 
00021 namespace observers
00022 {
00023 
00024 NearestForward::NearestForward(art_observers::ObserversConfig &config):
00025   Observer(config,
00026            art_msgs::Observation::Nearest_forward,
00027            std::string("Nearest_forward"))
00028 {
00029   distance_filter_.configure();
00030   velocity_filter_.configure();
00031 }
00032 
00033 NearestForward::~NearestForward() 
00034 {
00035 }
00036 
00037 // \brief Updates the message with new data received.
00038 art_msgs::Observation
00039   NearestForward::update(const art_msgs::ArtLanes &local_map,
00040                          const art_msgs::ArtLanes &obstacles,
00041                          MapPose pose_)
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   // get quadrilaterals ahead in the current lane
00051   art_msgs::ArtLanes lane_quads =
00052     quad_ops::filterLanes(robot_quad1, local_map,
00053                           *quad_ops::compare_forward_seg_lane);
00054 
00055   // get obstacles ahead in the current lane
00056   art_msgs::ArtLanes lane_obstacles =
00057     quad_ops::filterLanes(robot_quad1, obstacles,
00058                           *quad_ops::compare_forward_seg_lane);
00059 
00060   float distance = std::numeric_limits<float>::infinity();
00061   if (lane_obstacles.polygons.size()!=0)
00062     {
00063       // Get distance along road from robot to nearest obstacle
00064       int target_id = lane_obstacles.polygons[0].poly_id;
00065       distance = 0;
00066       for (size_t i = 0; i < lane_quads.polygons.size(); i++)
00067         {
00068           distance += lane_quads.polygons[i].length;
00069           if (lane_quads.polygons[i].poly_id == target_id)
00070             break;
00071         }
00072     }
00073 
00074   // Filter the distance by averaging over time
00075   float filt_distance;
00076   distance_filter_.update(distance, filt_distance);
00077   
00078   // Calculate velocity of object (including filter)
00079   float prev_distance = observation_.distance;
00080   ros::Time current_update(ros::Time::now());
00081   double time_change = (current_update - prev_update_).toSec();
00082   float velocity = (filt_distance - prev_distance) / (time_change);
00083   float filt_velocity;
00084   velocity_filter_.update(velocity,filt_velocity);
00085   prev_update_ = current_update; // Reset prev_update time
00086 
00087   // Time to intersection (infinite if obstacle moving away)
00088   double time = std::numeric_limits<float>::infinity();
00089 
00090   if (filt_velocity < 0)
00091     {
00092       // Object getting closer
00093       if (filt_velocity > -0.1)
00094         {
00095           // avoid dividing by a tiny number
00096           filt_velocity = 0.1;
00097         }
00098       time = fabs(filt_distance / filt_velocity);
00099     }
00100 
00101   // return the observation
00102   observation_.distance = filt_distance;
00103   observation_.velocity = filt_velocity;
00104   observation_.time = time;
00105   observation_.clear =  (time > 10.0);
00106   observation_.applicable = (velocity_filter_.isFull());
00107                    
00108   return observation_;
00109 }
00110 
00111 }; // namespace observers


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