$search
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