virtual_sensor_node.hpp
Go to the documentation of this file.
00001 /*
00002  * virtual_sensor_node.hpp
00003  *
00004  *  Created on: May 13, 2013
00005  *      Author: jorge
00006  */
00007 
00008 #ifndef VIRTUAL_SENSOR_NODE_HPP_
00009 #define VIRTUAL_SENSOR_NODE_HPP_
00010 
00011 #include <tf/tf.h>
00012 #include <tf/transform_listener.h>
00013 #include <sensor_msgs/LaserScan.h>
00014 
00015 #include <yocs_math_toolkit/geometry.hpp>
00016 
00017 #include <yocs_msgs/Wall.h>
00018 #include <yocs_msgs/WallList.h>
00019 #include <yocs_msgs/Column.h>
00020 #include <yocs_msgs/ColumnList.h>
00021 
00022 namespace virtual_sensor
00023 {
00024 
00025 class VirtualSensorNode
00026 {
00027 public:
00028 
00029   class Obstacle
00030   {
00031   public:
00032     Obstacle(const std::string& name, const tf::Transform& tf, double height)
00033       : name_(name), tf_(tf), height_(height) { }
00034     ~Obstacle() { }
00035     std::string& name() { return name_;}
00036     double distance()  { return distance_; }
00037     double minHeight() { return tf_.getOrigin().z(); }
00038     double maxHeight() { return tf_.getOrigin().z() + height_; }
00039     virtual bool intersects(double rx, double ry, double max_dist, double& distance) = 0;
00040 
00041   protected:
00042     std::string name_;
00043     tf::Transform tf_;
00044     double distance_;
00045     double height_;
00046   };
00047 
00048   class Column : public Obstacle
00049   {
00050   public:
00051     Column(const std::string& name, const tf::Transform& tf, double radius, double height)
00052       : Obstacle(name, tf, height)
00053     {
00054       radius_   = radius;
00055       distance_ = mtk::distance2D(tf.getOrigin()) - radius;
00056     }
00057 
00058     double radius() { return radius_; }
00059     bool intersects(double rx, double ry, double max_dist, double& distance)
00060     {
00061       double ix, iy;
00062       bool intersects =
00063           mtk::rayCircleIntersection(rx, ry, tf_.getOrigin().x(), tf_.getOrigin().y(), radius_, ix, iy, distance);
00064       if ((intersects == false) || (distance > max_dist))
00065         return false;
00066       else
00067         return true;
00068     }
00069 
00070   private:
00071     double radius_;
00072   };
00073 
00074   class Wall : public Obstacle
00075   {
00076   public:
00077     Wall(const std::string& name, const tf::Transform& tf, double length, double width, double height)
00078       : Obstacle(name, tf, height)
00079     {
00080       length_ = length;
00081       width_  = width;
00082 
00083       p1_.setX(- width_/2.0);
00084       p1_.setY(- length_/2.0);
00085       p1_.setZ(  0.0);
00086       p2_.setX(+ width_/2.0);
00087       p2_.setY(+ length_/2.0);
00088       p2_.setZ(+ height_);
00089 
00090       p1_ = tf_ * p1_;
00091       p2_ = tf_ * p2_;
00092 
00093       distance_ = mtk::pointSegmentDistance(0.0, 0.0, p1_.x(), p1_.y(), p2_.x(), p2_.y());
00094     }
00095 
00096     double length() { return length_; }
00097     double width()  { return width_;  }
00098     bool intersects(double rx, double ry, double max_dist, double& distance)
00099     {
00100       double ix, iy;
00101       bool intersects =
00102           mtk::raySegmentIntersection(0.0, 0.0, rx, ry, p1_.x(), p1_.y(), p2_.x(), p2_.y(), ix, iy, distance);
00103       if ((intersects == false) || (distance > max_dist))
00104         return false;
00105       else
00106         return true;
00107     }
00108 
00109   private:
00110     tf::Point p1_;
00111     tf::Point p2_;
00112     double length_;
00113     double width_;
00114   };
00115 
00116   VirtualSensorNode();
00117   ~VirtualSensorNode();
00118 
00119   bool init();
00120   void spin();
00121 
00122   void columnPosesCB(const yocs_msgs::ColumnList::ConstPtr& msg);
00123   void wallPosesCB(const yocs_msgs::WallList::ConstPtr& msg);
00124 
00125 private:
00126   double          angle_min_;
00127   double          angle_max_;
00128   double          angle_inc_;
00129   double          scan_time_;
00130   double          range_min_;
00131   double          range_max_;
00132   double          frequency_;
00133   int             hits_count_;
00134   std::string     sensor_frame_id_;   
00135   std::string     global_frame_id_;
00136   ros::Publisher  virtual_obs_pub_;
00137   ros::Subscriber wall_poses_sub_;
00138   ros::Subscriber column_poses_sub_;
00139   ros::Subscriber table_poses_sub_;
00140   sensor_msgs::LaserScan     scan_;
00141   tf::TransformListener tf_listener_;
00142   std::vector<yocs_msgs::Column> columns_;
00143   std::vector<yocs_msgs::Wall>   walls_;
00144 
00153   bool add(boost::shared_ptr<Obstacle>& new_obs, std::vector< boost::shared_ptr<Obstacle> >& obstacles);
00154 };
00155 
00156 } /* namespace virtual_sensor */
00157 #endif /* VIRTUAL_SENSOR_NODE_HPP_ */


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