range_sensor_layer.cpp
Go to the documentation of this file.
00001 #include <range_sensor_layer/range_sensor_layer.h>
00002 #include <boost/algorithm/string.hpp>
00003 #include <pluginlib/class_list_macros.h>
00004 #include <angles/angles.h>
00005 
00006 PLUGINLIB_EXPORT_CLASS(range_sensor_layer::RangeSensorLayer, costmap_2d::Layer)
00007 
00008 using costmap_2d::NO_INFORMATION;
00009 
00010 namespace range_sensor_layer
00011 {
00012 
00013 RangeSensorLayer::RangeSensorLayer() {}
00014 
00015 void RangeSensorLayer::onInitialize()
00016 {
00017   ros::NodeHandle nh("~/" + name_);
00018   current_ = true;
00019   buffered_readings_ = 0;
00020   last_reading_time_ = ros::Time::now();
00021   default_value_ = to_cost(0.5);
00022 
00023   matchSize();
00024   min_x_ = min_y_ = -std::numeric_limits<double>::max();
00025   max_x_ = max_y_ = std::numeric_limits<double>::max();
00026 
00027   // Default topic names list contains a single topic: /sonar
00028   // We use the XmlRpcValue constructor that takes a XML string and reading start offset
00029   const char* xml = "<value><array><data><value>/sonar</value></data></array></value>";
00030   int zero_offset = 0;
00031   std::string topics_ns;
00032   XmlRpc::XmlRpcValue topic_names(xml, &zero_offset);
00033 
00034   nh.param("ns", topics_ns, std::string());
00035   nh.param("topics", topic_names, topic_names);
00036 
00037   InputSensorType input_sensor_type = ALL;
00038   std::string sensor_type_name;
00039   nh.param("input_sensor_type", sensor_type_name, std::string("ALL"));
00040 
00041   boost::to_upper(sensor_type_name);
00042   ROS_INFO("%s: %s as input_sensor_type given", name_.c_str(), sensor_type_name.c_str());
00043 
00044   if (sensor_type_name == "VARIABLE")
00045     input_sensor_type = VARIABLE;
00046   else if (sensor_type_name == "FIXED")
00047     input_sensor_type = FIXED;
00048   else if (sensor_type_name == "ALL")
00049     input_sensor_type = ALL;
00050   else
00051   {
00052     ROS_ERROR("%s: Invalid input sensor type: %s", name_.c_str(), sensor_type_name.c_str());
00053   }
00054 
00055   // Validate topic names list: it must be a (normally non-empty) list of strings
00056   if ((topic_names.valid() == false) || (topic_names.getType() != XmlRpc::XmlRpcValue::TypeArray))
00057   {
00058     ROS_ERROR("Invalid topic names list: it must be a non-empty list of strings");
00059     return;
00060   }
00061 
00062   if (topic_names.size() < 1)
00063   {
00064     // This could be an error, but I keep it as it can be useful for debug
00065     ROS_WARN("Empty topic names list: range sensor layer will have no effect on costmap");
00066   }
00067 
00068   // Traverse the topic names list subscribing to all of them with the same callback method
00069   for (unsigned int i = 0; i < topic_names.size(); i++)
00070   {
00071     if (topic_names[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00072     {
00073       ROS_WARN("Invalid topic names list: element %d is not a string, so it will be ignored", i);
00074     }
00075     else
00076     {
00077       std::string topic_name(topics_ns);
00078       if ((topic_name.size() > 0) && (topic_name.at(topic_name.size() - 1) != '/'))
00079         topic_name += "/";
00080       topic_name += static_cast<std::string>(topic_names[i]);
00081 
00082       if (input_sensor_type == VARIABLE)
00083         processRangeMessageFunc_ = boost::bind(&RangeSensorLayer::processVariableRangeMsg, this, _1);
00084       else if (input_sensor_type == FIXED)
00085         processRangeMessageFunc_ = boost::bind(&RangeSensorLayer::processFixedRangeMsg, this, _1);
00086       else if (input_sensor_type == ALL)
00087         processRangeMessageFunc_ = boost::bind(&RangeSensorLayer::processRangeMsg, this, _1);
00088       else
00089       {
00090         ROS_ERROR(
00091             "%s: Invalid input sensor type: %s. Did you make a new type and forgot to choose the subscriber for it?",
00092             name_.c_str(), sensor_type_name.c_str());
00093       }
00094 
00095       range_subs_.push_back(nh.subscribe(topic_name, 100, &RangeSensorLayer::bufferIncomingRangeMsg, this));
00096 
00097       ROS_INFO("RangeSensorLayer: subscribed to topic %s", range_subs_.back().getTopic().c_str());
00098     }
00099   }
00100 
00101   dsrv_ = new dynamic_reconfigure::Server<range_sensor_layer::RangeSensorLayerConfig>(nh);
00102   dynamic_reconfigure::Server<range_sensor_layer::RangeSensorLayerConfig>::CallbackType cb = boost::bind(
00103       &RangeSensorLayer::reconfigureCB, this, _1, _2);
00104   dsrv_->setCallback(cb);
00105   global_frame_ = layered_costmap_->getGlobalFrameID();
00106 }
00107 
00108 
00109 double RangeSensorLayer::gamma(double theta)
00110 {
00111     if(fabs(theta)>max_angle_)
00112         return 0.0;
00113     else
00114         return 1 - pow(theta/max_angle_, 2);
00115 }
00116 
00117 double RangeSensorLayer::delta(double phi)
00118 {
00119     return 1 - (1+tanh(2*(phi-phi_v_)))/2;
00120 }
00121 
00122 void RangeSensorLayer::get_deltas(double angle, double *dx, double *dy)
00123 {
00124     double ta = tan(angle);
00125     if(ta==0)
00126         *dx = 0;
00127     else
00128         *dx = resolution_ / ta;
00129 
00130     *dx = copysign(*dx, cos(angle));
00131     *dy = copysign(resolution_, sin(angle));
00132 }
00133 
00134 double RangeSensorLayer::sensor_model(double r, double phi, double theta)
00135 {
00136     double lbda = delta(phi)*gamma(theta);
00137 
00138     double delta = resolution_;
00139 
00140     if(phi >= 0.0 and phi < r - 2 * delta * r)
00141         return (1- lbda) * (0.5);
00142     else if(phi < r - delta * r)
00143         return lbda* 0.5 * pow((phi - (r - 2*delta*r))/(delta*r), 2)+(1-lbda)*.5;
00144     else if(phi < r + delta * r){
00145         double J = (r-phi)/(delta*r);
00146         return lbda * ((1-(0.5)*pow(J,2)) -0.5) + 0.5;
00147     }
00148     else
00149         return 0.5;
00150 }
00151 
00152 
00153 void RangeSensorLayer::reconfigureCB(range_sensor_layer::RangeSensorLayerConfig &config, uint32_t level)
00154 {
00155   phi_v_ = config.phi;
00156   max_angle_ = config.max_angle;
00157   no_readings_timeout_ = config.no_readings_timeout;
00158   clear_threshold_ = config.clear_threshold;
00159   mark_threshold_ = config.mark_threshold;
00160   clear_on_max_reading_ = config.clear_on_max_reading;
00161     
00162   if(enabled_ != config.enabled)
00163   {
00164     enabled_ = config.enabled;
00165     current_ = false;
00166   }
00167 }
00168 
00169 void RangeSensorLayer::bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& range_message)
00170 {
00171   boost::mutex::scoped_lock lock(range_message_mutex_);
00172   range_msgs_buffer_.push_back(*range_message);
00173 }
00174 
00175 void RangeSensorLayer::updateCostmap()
00176 {
00177   std::list<sensor_msgs::Range> range_msgs_buffer_copy;
00178 
00179   range_message_mutex_.lock();
00180   range_msgs_buffer_copy = std::list<sensor_msgs::Range>(range_msgs_buffer_);
00181   range_msgs_buffer_.clear();
00182   range_message_mutex_.unlock();
00183 
00184   for (std::list<sensor_msgs::Range>::iterator range_msgs_it = range_msgs_buffer_copy.begin();
00185       range_msgs_it != range_msgs_buffer_copy.end(); range_msgs_it++)
00186   {
00187     processRangeMessageFunc_(*range_msgs_it);
00188   }
00189 }
00190 
00191 void RangeSensorLayer::processRangeMsg(sensor_msgs::Range& range_message)
00192 {
00193   if (range_message.min_range == range_message.max_range)
00194     processFixedRangeMsg(range_message);
00195   else
00196     processVariableRangeMsg(range_message);
00197 }
00198 
00199 void RangeSensorLayer::processFixedRangeMsg(sensor_msgs::Range& range_message)
00200 {
00201   if (!isinf(range_message.range))
00202   {
00203     ROS_ERROR_THROTTLE(1.0,
00204         "Fixed distance ranger (min_range == max_range) in frame %s sent invalid value. Only -Inf (== object detected) and Inf (== no object detected) are valid.",
00205         range_message.header.frame_id.c_str());
00206     return;
00207   }
00208 
00209   bool clear_sensor_cone = false;
00210 
00211   if (range_message.range > 0) //+inf
00212   {
00213     if (!clear_on_max_reading_)
00214       return; //no clearing at all
00215 
00216     clear_sensor_cone = true;
00217   }
00218 
00219   range_message.range = range_message.min_range;
00220 
00221   updateCostmap(range_message, clear_sensor_cone);
00222 }
00223 
00224 void RangeSensorLayer::processVariableRangeMsg(sensor_msgs::Range& range_message)
00225 {
00226   if (range_message.range < range_message.min_range || range_message.range > range_message.max_range)
00227     return;
00228 
00229   bool clear_sensor_cone = false;
00230 
00231   if (range_message.range == range_message.max_range && clear_on_max_reading_)
00232     clear_sensor_cone = true;
00233 
00234   updateCostmap(range_message, clear_sensor_cone);
00235 }
00236 
00237 void RangeSensorLayer::updateCostmap(sensor_msgs::Range& range_message, bool clear_sensor_cone)
00238 {
00239   max_angle_ = range_message.field_of_view/2;
00240 
00241   geometry_msgs::PointStamped in, out;
00242   in.header.stamp = range_message.header.stamp;
00243   in.header.frame_id = range_message.header.frame_id;
00244 
00245   if(!tf_->waitForTransform(global_frame_, in.header.frame_id,
00246         in.header.stamp, ros::Duration(0.1)) ) {
00247      ROS_ERROR_THROTTLE(1.0, "Range sensor layer can't transform from %s to %s at %f",
00248         global_frame_.c_str(), in.header.frame_id.c_str(),
00249         in.header.stamp.toSec());
00250      return;
00251   }
00252 
00253   tf_->transformPoint (global_frame_, in, out);
00254 
00255   double ox = out.point.x, oy = out.point.y;
00256 
00257   in.point.x = range_message.range;
00258 
00259   tf_->transformPoint(global_frame_, in, out);
00260 
00261   double tx = out.point.x, ty = out.point.y;
00262 
00263   // calculate target props
00264   double dx = tx-ox, dy = ty-oy,
00265         theta = atan2(dy,dx), d = sqrt(dx*dx+dy*dy);
00266 
00267   // Integer Bounds of Update
00268   int bx0, by0, bx1, by1;
00269 
00270   // Bounds includes the origin
00271   worldToMapNoBounds(ox, oy, bx0, by0);
00272   bx1 = bx0;
00273   by1 = by0;
00274   touch(ox, oy, &min_x_, &min_y_, &max_x_, &max_y_);
00275 
00276   // Update Map with Target Point
00277   unsigned int aa, ab;
00278   if(worldToMap(tx, ty, aa, ab)){
00279     setCost(aa, ab, 233);
00280     touch(tx, ty, &min_x_, &min_y_, &max_x_, &max_y_);
00281   }
00282 
00283   double mx, my;
00284   int a, b;
00285 
00286   // Update left side of sonar cone
00287   mx = ox + cos(theta-max_angle_) * d * 1.2;
00288   my = oy + sin(theta-max_angle_) * d * 1.2;
00289   worldToMapNoBounds(mx, my, a, b);
00290   bx0 = std::min(bx0, a);
00291   bx1 = std::max(bx1, a);
00292   by0 = std::min(by0, b);
00293   by1 = std::max(by1, b);
00294   touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);
00295 
00296   // Update right side of sonar cone
00297   mx = ox + cos(theta+max_angle_) * d * 1.2;
00298   my = oy + sin(theta+max_angle_) * d * 1.2;
00299 
00300   worldToMapNoBounds(mx, my, a, b);
00301   bx0 = std::min(bx0, a);
00302   bx1 = std::max(bx1, a);
00303   by0 = std::min(by0, b);
00304   by1 = std::max(by1, b);
00305   touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);
00306 
00307   // Limit Bounds to Grid
00308   bx0 = std::max(0, bx0);
00309   by0 = std::max(0, by0);
00310   bx1 = std::min((int)size_x_, bx1);
00311   by1 = std::min((int)size_y_, by1);
00312 
00313   for(unsigned int x=bx0; x<=(unsigned int)bx1; x++){
00314     for(unsigned int y=by0; y<=(unsigned int)by1; y++){
00315       double wx, wy;
00316       mapToWorld(x,y,wx,wy);
00317       update_cell(ox, oy, theta, range_message.range, wx, wy, clear_sensor_cone);
00318     }
00319   }
00320 
00321   buffered_readings_++;
00322   last_reading_time_ = ros::Time::now();
00323 }
00324 
00325 void RangeSensorLayer::update_cell(double ox, double oy, double ot, double r, double nx, double ny, bool clear)
00326 {
00327   unsigned int x, y;
00328   if(worldToMap(nx, ny, x, y)){
00329     double dx = nx-ox, dy = ny-oy;
00330     double theta = atan2(dy, dx) - ot;
00331     theta = angles::normalize_angle(theta);
00332     double phi = sqrt(dx*dx+dy*dy);
00333     double sensor = 0.0;
00334     if(!clear)
00335         sensor = sensor_model(r,phi,theta);
00336     double prior = to_prob(getCost(x,y));
00337     double prob_occ = sensor * prior;
00338     double prob_not = (1 - sensor) * (1 - prior);
00339     double new_prob = prob_occ/(prob_occ+prob_not);
00340 
00341     //ROS_INFO("%f %f | %f %f = %f", dx, dy, theta, phi, sensor);
00342     //ROS_INFO("%f | %f %f | %f", prior, prob_occ, prob_not, new_prob);
00343       unsigned char c = to_cost(new_prob);
00344       setCost(x,y,c);
00345   }
00346 }
00347 
00348 void RangeSensorLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
00349                                            double* min_y, double* max_x, double* max_y)
00350 {
00351   if (layered_costmap_->isRolling())
00352     updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
00353 
00354   updateCostmap();
00355 
00356   *min_x = std::min(*min_x, min_x_);
00357   *min_y = std::min(*min_y, min_y_);
00358   *max_x = std::max(*max_x, max_x_);
00359   *max_y = std::max(*max_y, max_y_);
00360 
00361   min_x_ = min_y_ = std::numeric_limits<double>::max();
00362   max_x_ = max_y_ = std::numeric_limits<double>::min();
00363 
00364   if (!enabled_)
00365   {
00366     current_ = true;
00367     return;
00368   }
00369   
00370   if (buffered_readings_ == 0)
00371   {
00372     if (no_readings_timeout_ > 0.0 &&
00373         (ros::Time::now() - last_reading_time_).toSec() > no_readings_timeout_)
00374     {
00375       ROS_WARN_THROTTLE(2.0, "No range readings received for %.2f seconds, " \
00376                              "while expected at least every %.2f seconds.",
00377                (ros::Time::now() - last_reading_time_).toSec(), no_readings_timeout_);
00378       current_ = false;
00379     }
00380   }
00381 
00382 }
00383 
00384 void RangeSensorLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
00385                                           int max_j)
00386 {
00387   if (!enabled_)
00388     return;
00389 
00390   unsigned char* master_array = master_grid.getCharMap();
00391   unsigned int span = master_grid.getSizeInCellsX();
00392   unsigned char clear = to_cost(clear_threshold_), mark = to_cost(mark_threshold_);
00393 
00394   for (int j = min_j; j < max_j; j++)
00395   {
00396     unsigned int it = j * span + min_i;
00397     for (int i = min_i; i < max_i; i++)
00398     {
00399       unsigned char prob = costmap_[it];
00400       unsigned char current;
00401       if(prob==costmap_2d::NO_INFORMATION){
00402         it++;
00403         continue;
00404       }
00405       else if(prob>mark)
00406         current = costmap_2d::LETHAL_OBSTACLE;
00407       else if(prob<clear)
00408         current = costmap_2d::FREE_SPACE;
00409       else{
00410         it++;
00411         continue;
00412       }
00413 
00414       unsigned char old_cost = master_array[it];
00415 
00416       if (old_cost == NO_INFORMATION || old_cost < current)
00417         master_array[it] = current;
00418       it++;
00419     }
00420   }
00421 
00422   buffered_readings_ = 0;
00423   current_ = true;
00424 }
00425 
00426 void RangeSensorLayer::reset()
00427 {
00428   ROS_DEBUG("Reseting range sensor layer...");
00429   deactivate();
00430   resetMaps();
00431   current_ = true;
00432   activate();
00433 }
00434 
00435 void RangeSensorLayer::deactivate()
00436 {
00437   range_msgs_buffer_.clear();
00438 }
00439 
00440 void RangeSensorLayer::activate()
00441 {
00442   range_msgs_buffer_.clear();
00443 }
00444 
00445 } // end namespace


range_sensor_layer
Author(s): David!!
autogenerated on Tue Dec 27 2016 03:52:43