virtual_sensor_node.cpp
Go to the documentation of this file.
00001 /*
00002  * virtual_sensor_node.cpp
00003  *
00004  *  Created on: May 13, 2013
00005  *      Author: jorge
00006  */
00007 
00008 #include "yocs_virtual_sensor/virtual_sensor_node.hpp"
00009 
00010 
00011 namespace virtual_sensor
00012 {
00013 
00014 VirtualSensorNode::VirtualSensorNode()
00015 {
00016 }
00017 
00018 VirtualSensorNode::~VirtualSensorNode()
00019 {
00020 }
00021 
00022 
00023 bool VirtualSensorNode::init()
00024 {
00025   ros::NodeHandle nh;
00026   ros::NodeHandle pnh("~");
00027 
00028   int samples;
00029 
00030   pnh.param("samples",    samples,    360);
00031   pnh.param("angle_min",  angle_min_, -M_PI);
00032   pnh.param("angle_max",  angle_max_, +M_PI);
00033   pnh.param("range_min",  range_min_, 0.0);
00034   pnh.param("range_max",  range_max_, std::numeric_limits<double>::quiet_NaN());
00035   pnh.param("frequency",  frequency_, std::numeric_limits<double>::quiet_NaN());
00036   pnh.param("hits_count", hits_count_, 2);
00037   pnh.param("sensor_frame", sensor_frame_id_, std::string("/base_link"));
00038   pnh.param("global_frame", global_frame_id_, std::string("/map"));
00039 
00040   double costmap_update_frequency, costmap_width, costmap_height;
00041 
00042   nh.getParam("move_base/local_costmap/update_frequency", costmap_update_frequency);
00043   nh.getParam("move_base/local_costmap/width",            costmap_width);
00044   nh.getParam("move_base/local_costmap/height",           costmap_height);
00045 
00046   // Use local costmap configuration to decide some default values
00047   if (std::isnan(range_max_) == true)
00048     range_max_ = std::max(costmap_width, costmap_height)/2.0;
00049 
00050   if (std::isnan(frequency_) == true)
00051     frequency_ = costmap_update_frequency*2.0;
00052 
00053   column_poses_sub_ = nh.subscribe("column_pose_list", 1, &VirtualSensorNode::columnPosesCB, this);
00054   wall_poses_sub_   = nh.subscribe("wall_pose_list",   1, &VirtualSensorNode::wallPosesCB, this);
00055   virtual_obs_pub_  = nh.advertise <sensor_msgs::LaserScan> ("virtual_sensor_scan", 1, true);
00056 
00057   // Set virtual scan configuration
00058   scan_.header.frame_id = sensor_frame_id_;
00059   scan_.angle_min = angle_min_;
00060   scan_.angle_max = angle_max_;
00061   scan_.angle_increment = (angle_max_ - angle_min_)/samples;
00062   scan_.scan_time = 1.0 / frequency_;
00063   scan_.range_min = range_min_;
00064   scan_.range_max = range_max_;
00065   scan_.ranges.resize(samples);
00066 
00067   return true;
00068 }
00069 
00070 void VirtualSensorNode::columnPosesCB(const yocs_msgs::ColumnList::ConstPtr& msg)
00071 {
00072   columns_ = msg->obstacles;
00073 }
00074 
00075 void VirtualSensorNode::wallPosesCB(const yocs_msgs::WallList::ConstPtr& msg)
00076 {
00077   walls_ = msg->obstacles;
00078 }
00079 
00080 
00081 void VirtualSensorNode::spin()
00082 {
00083   ros::Rate rate(frequency_);
00084 
00085   while (ros::ok())
00086   {
00087     if ((virtual_obs_pub_.getNumSubscribers() > 0) && ((columns_.size() > 0) || (walls_.size() > 0)))
00088     {
00089       scan_.header.stamp = ros::Time::now();
00090 
00091       tf::StampedTransform robot_gb;
00092       try
00093       {
00094         tf_listener_.lookupTransform(global_frame_id_, sensor_frame_id_, ros::Time(0.0), robot_gb);
00095       }
00096       catch (tf::TransformException& e)
00097       {
00098         // This can happen during startup, while some part of the localization chain is missing
00099         // If not, probably sensor_frame is missconfigured
00100         ROS_WARN_THROTTLE(2, "Cannot get tf %s -> %s: %s",
00101                           global_frame_id_.c_str(), sensor_frame_id_.c_str(), e.what());
00102         continue;
00103       }
00104       tf::Transform robot_gb_inv = robot_gb.inverse();
00105 
00106       // Pre-filter obstacles:
00107       //  - put in sensor reference system
00108       //  - remove those out of range
00109       //  - short by increasing distance to the robot
00110       std::vector< boost::shared_ptr<Obstacle> > obstacles;
00111 
00112       // Column obstacles
00113       for (unsigned int i = 0; i < columns_.size(); i++)
00114       {
00115         tf::Transform obs_abs_tf;
00116         tf::poseMsgToTF(columns_[i].pose.pose.pose, obs_abs_tf);
00117         obs_abs_tf.setRotation(tf::Quaternion::getIdentity());
00118         tf::Transform obs_tf = robot_gb_inv * obs_abs_tf;
00119         boost::shared_ptr<Obstacle> new_obs(new Column(columns_[i].name, obs_tf, columns_[i].radius, columns_[i].height));
00120 
00121         add(new_obs, obstacles);
00122       }
00123 
00124       // Wall obstacles
00125       for (unsigned int i = 0; i < walls_.size(); i++)
00126       {
00127         tf::Transform obs_abs_tf;
00128         tf::poseMsgToTF(walls_[i].pose.pose.pose, obs_abs_tf);
00129         tf::Transform obs_tf = robot_gb_inv * obs_abs_tf;
00130         boost::shared_ptr<Obstacle> new_obs(new Wall(walls_[i].name, obs_tf, walls_[i].length, walls_[i].width, walls_[i].height));
00131 
00132         add(new_obs, obstacles);
00133       }
00134 
00135       int ray = 0;
00136 
00137       // Fire sensor "rays" and register closest hit, if any. We test hits_count_ consecutive objects; in most
00138       // cases 2 hits should be enough, although in some cases three or more ill-arranged obstacles will return
00139       // a wrong shortest distance (remember that obstacles are shorted by its closest distance to the robot)
00140       for (double ray_theta = scan_.angle_min; ray_theta <= scan_.angle_max; ray_theta += scan_.angle_increment)
00141       {
00142         double rx = scan_.range_max*std::cos(ray_theta);
00143         double ry = scan_.range_max*std::sin(ray_theta);
00144 
00145         int hits = 0;
00146         scan_.ranges[ray] = scan_.range_max;
00147 
00148         for (unsigned int i = 0; i < obstacles.size(); i++)
00149         {
00150           double distance;
00151           if (obstacles[i]->intersects(rx, ry, scan_.range_max, distance) == true)
00152           {
00153             // Hit; take the shortest distance until now and keep checking until we reach hits_count_
00154             scan_.ranges[ray] = std::min(scan_.ranges[ray], (float)distance);
00155 
00156             if (hits < hits_count_)
00157             {
00158               hits++;
00159             }
00160             else
00161             {
00162               break;   // enough hits
00163             }
00164           }
00165         }
00166 
00167         ray++;
00168       }
00169 
00170       virtual_obs_pub_.publish(scan_);
00171     }
00172 
00173     ros::spinOnce();
00174     rate.sleep();
00175   }
00176 }
00177 
00178 bool VirtualSensorNode::add(boost::shared_ptr<Obstacle>& new_obs, std::vector< boost::shared_ptr<Obstacle> >& obstacles)
00179 {
00180   if ((new_obs->minHeight() > 0.0) || (new_obs->maxHeight() < 0.0))
00181   {
00182     return false;  // Obstacle above/below the virtual sensor (who is a 2D scan)
00183   }
00184 
00185   if (new_obs->distance() <= 0.0)
00186   {
00187     ROS_WARN_THROTTLE(2, "The robot is inside an obstacle??? Ignore it (distance: %f)", new_obs->distance());
00188     return false;
00189   }
00190 
00191   if (new_obs->distance() <= scan_.range_max)
00192   {
00193     std::vector< boost::shared_ptr<Obstacle> >::iterator it;
00194     for (it = obstacles.begin(); it != obstacles.end(); ++it)
00195     {
00196       if (new_obs->distance() <= (*it)->distance())
00197       {
00198         obstacles.insert(it, new_obs);
00199         return true;
00200       }
00201     }
00202 
00203     if ((obstacles.size() == 0) || it == obstacles.end())
00204     {
00205       // This obstacle is the first inserted or is the farthest from the robot
00206       obstacles.push_back(new_obs);
00207       return true;
00208     }
00209   }
00210 
00211   return false;
00212 }
00213 
00214 } // namespace virtual_sensor
00215 
00216 
00217 int main(int argc, char **argv)
00218 {
00219   ros::init(argc, argv, "virtual_sensor");
00220 
00221   virtual_sensor::VirtualSensorNode node;
00222   if (node.init() == false)
00223   {
00224     ROS_ERROR("%s initialization failed", ros::this_node::getName().c_str());
00225     return -1;
00226   }
00227   ROS_INFO("%s initialized", ros::this_node::getName().c_str());
00228   node.spin();
00229 
00230   return 0;
00231 }


yocs_virtual_sensor
Author(s): Jorge Santos
autogenerated on Fri Aug 28 2015 13:45:16