RangeSensor.cpp
Go to the documentation of this file.
00001 #include "RangeSensor.h"
00002 
00003 
00004 #include <ros/ros.h>
00005 
00006 
00007 extern volatile bool g_stopall;
00008 
00009 #include <sensor_msgs/point_cloud_conversion.h>
00010 #include <pcl/ros/conversions.h>
00011 
00012 
00013 
00014 #define XML_PROPERTY_TOPIC_SR4 "Topic"
00015 #define XML_PROPERTY_POINT_CLOUD2 "PointCloud2"
00016 #define XML_PROPERTY_INTEGRATE "Integrate"
00017 
00018 #define XML_PROPERTY_FOCALLENGTH "focal_length"
00019 #define XML_PROPERTY_DISTPARAM "dist_param"
00020 #define XML_PROPERTY_PIXWIDTH "sx"
00021 #define XML_PROPERTY_PIXHEIGHT "sy"
00022 #define XML_PROPERTY_CX "cx"
00023 #define XML_PROPERTY_CY "cy"
00024 #define XML_PROPERTY_WIDTH "width"
00025 #define XML_PROPERTY_HEIGHT "height"
00026 
00027 #define XML_PROPERTY_LINKPARAM_NAMESPACE "param_ns"
00028 
00029 using namespace cop;
00030 
00031 RangeSensor::RangeSensor() :
00032     m_grabbing(false),
00033     m_bintegration(true),
00034     m_subscribePointCloud2(false),
00035     m_self_filter(NULL)
00036 {
00037   printf("Successfully initialized plugin class RangeSensor\n");
00038   m_max_cameraImages = 1;
00039 }
00040 
00041 
00042 RangeSensor::~RangeSensor()
00043 {
00044   Stop();
00045   if(m_self_filter != NULL)
00046     delete m_self_filter;
00047 }
00048 
00052 void RangeSensor::SetData(cop::XMLTag* tag)
00053 {
00054   Sensor::SetData(tag);
00055   printf("Set data\n");
00056   m_stTopic = tag->GetProperty(XML_PROPERTY_TOPIC_SR4, "cloud_sr");
00057   m_subscribePointCloud2  = tag->GetPropertyInt(XML_PROPERTY_POINT_CLOUD2, 0) != 0;
00058   m_bintegration = tag->GetPropertyInt(XML_PROPERTY_INTEGRATE, 1) == 1;
00059   m_calibration.resize(8);
00060   m_calibration[0] = tag->GetPropertyDouble(XML_PROPERTY_FOCALLENGTH, 0.0105631);
00061   m_calibration[1] = tag->GetPropertyDouble(XML_PROPERTY_DISTPARAM , -8623.59);
00062   m_calibration[2] = tag->GetPropertyDouble(XML_PROPERTY_PIXWIDTH,  3.99851e-05);
00063   m_calibration[3] = tag->GetPropertyDouble(XML_PROPERTY_PIXHEIGHT, 4e-05);
00064   m_calibration[4] = tag->GetPropertyDouble(XML_PROPERTY_CX, 87.8255);
00065   m_calibration[5] = tag->GetPropertyDouble(XML_PROPERTY_CY, 71.9162);
00066   m_calibration[6] = tag->GetPropertyDouble(XML_PROPERTY_WIDTH, 176.0);
00067   m_calibration[7] = tag->GetPropertyDouble(XML_PROPERTY_HEIGHT, 144.0);
00068 
00069   m_paramNamespace = tag->GetProperty(XML_PROPERTY_LINKPARAM_NAMESPACE , "rosie_arm_navigation_tilt_laser_self_filter");
00070   ros::NodeHandle nh(m_paramNamespace);
00071   m_self_filter = new filters::SelfFilter<pcl::PointCloud<pcl::PointXYZ> >(nh);
00072 
00073 }
00074 
00075 cop::Reading*   RangeSensor::GetReading(const long &Frame)
00076 {
00077   if((signed)m_images.size() < (Frame - m_deletedOffset + 1) || m_images.size() == 0)
00078   {
00079     if(m_grabbing)
00080     {
00081       while(!g_stopall && m_grabbing && (m_images.size() == 0))
00082       {
00083         printf("waiting for the camera %s to start grabbing\n", GetSensorID().c_str());
00084         sleep(1);
00085       }
00086       printf("Got a new image: %d\n", (int)m_images.size());
00087     }
00088     if(m_images.size() == 0)
00089     {
00090       printf("unexpected error\n");
00091       throw "Asking for images from a camera that has no images";
00092     }
00093   }
00094   if(Frame == -1 || (Frame - m_deletedOffset < 0 && (unsigned)(Frame - m_deletedOffset) >= m_images.size()))
00095   {
00096     return GetReading_Lock(m_images.size() -1);
00097     /*return m_images[m_images.size() -1];*/
00098   }
00099   return GetReading_Lock(Frame - m_deletedOffset);
00100 }
00101 
00102 bool RangeSensor::CanSee(cop::RelPose &pose) const
00103 {
00104   double row, col;
00105   if(this->m_relPose != NULL)
00106   {
00107     if(this->m_relPose->m_uniqueID != pose.m_uniqueID)
00108     {
00109       Matrix m = pose.GetMatrix(this->m_relPose->m_uniqueID);
00110       ProjectPoint3DToSensor(m.element(0,3), m.element(1,3), m.element(2,3), row, col);
00111       printf("Project %f %f %f -> %f %f (< %f && < %f) \n", m.element(0,3), m.element(1,3), m.element(2,3), row, col, m_calibration[6], m_calibration[7]);
00112       if(row >= 0 && row < m_calibration[7] && col >= 0 && col < m_calibration[6])
00113       {
00114         return true;
00115       }
00116     }
00117     else
00118     {
00119       return true;
00120     }
00121   }
00122   return false;
00123 }
00124 
00125 Reading* RangeSensor::ApplySelfFilter(Reading* read)
00126 {
00127   /*
00128   printf("In self filter\n");
00129   m_self_filter->setSensorFrame(m_relPose->m_mapstring);
00130 
00131 
00132   robot_self_filter::SelfMask* mask = m_self_filter->getSelfMask ();
00133 
00134   std::vector<geometry_msgs::Point32>::iterator it = ((SwissRangerReading*)read)->m_image.points.begin();
00135   mask->assumeFrame(((SwissRangerReading*)read)->m_image.header.frame_id,((SwissRangerReading*)read)->m_image.header.stamp);
00136   int i = 0, j = 0;
00137   for (; it != ((SwissRangerReading*)read)->m_image.points.end(); i++)
00138   {
00139     btVector3 pt ((*it).x, (*it).y, (*it).z );
00140     int result = mask->getMaskIntersection(pt);
00141     if(result == robot_self_filter::OUTSIDE)
00142     {
00143       printf("Removeing element %d: result was %d\n", j++, result);
00144       it = ((SwissRangerReading*)read)->m_image.points.erase(it);
00145     }
00146     else
00147     {
00148       it++;
00149     }
00150     if(it == ((SwissRangerReading*)read)->m_image.points.end())
00151       break;
00152   }*/
00153   return read;
00154 }
00155 
00156 
00157 void RangeSensor::CallBack(const sensor_msgs::PointCloudConstPtr& cloud)
00158 {
00159   SwissRangerReading* reading;
00160   if(m_images.size() > 0)
00161   {
00162     if(m_bintegration)
00163     {
00164       SwissRangerReading* last_reading = (SwissRangerReading*)GetReading_Lock(m_images.size() - 1);
00165       reading = new SwissRangerReading(cloud, last_reading, GetRelPose());
00166       last_reading->Free();
00167     }
00168     else
00169       reading = new SwissRangerReading(cloud, NULL, NULL);
00170   }
00171   else
00172   {
00173     reading = new SwissRangerReading(cloud, NULL, NULL);
00174   }
00175 
00176   PushBack(reading);
00177   while(m_images.size() > m_max_cameraImages)
00178   {
00179     if(DeleteReading())
00180       continue;
00181     else
00182     {
00183       printf("SR: Could not delete an image!");
00184       break;
00185     }
00186   }
00187 }
00188 
00189 
00190 void RangeSensor::CallBackPCL2(const sensor_msgs::PointCloud2ConstPtr& cloud2)
00191 {
00192   SwissRangerReading* reading;
00193   sensor_msgs::PointCloud cloud;
00194   if(!sensor_msgs::convertPointCloud2ToPointCloud(*cloud2, cloud))
00195   {
00196     ROS_ERROR("Conversion of PointCloud to PointCloud2 failed\n");
00197     throw("Error in point cloud coversion\n");
00198     return;
00199   }
00200   if(m_images.size() > 0)
00201   {
00202     if(m_bintegration)
00203     {
00204       SwissRangerReading* last_reading = (SwissRangerReading*)GetReading_Lock(m_images.size() - 1);
00205       reading = new SwissRangerReading(cloud, last_reading, GetRelPose());
00206       last_reading->Free();
00207     }
00208     else
00209       reading = new SwissRangerReading(cloud, NULL, NULL);
00210   }
00211   else
00212   {
00213     reading = new SwissRangerReading(cloud, NULL, NULL);
00214   }
00215 
00216   PushBack(reading);
00217   while(m_images.size() > m_max_cameraImages)
00218   {
00219     if(DeleteReading())
00220       continue;
00221     else
00222     {
00223       printf("SR: Could not delete an image!");
00224       break;
00225     }
00226   }
00227 }
00228 
00229 std::pair<std::string, std::vector<double> > RangeSensor::GetUnformatedCalibrationValues() const
00230 {
00231   return std::pair<std::string, std::vector<double> >("HALCONCALIB", m_calibration);
00232 }
00233 
00234 
00239 bool    RangeSensor::Start()
00240 {
00241   ros::NodeHandle nh;
00242   printf("Subscribe to topic %s \n", m_stTopic.c_str());
00243   if(m_subscribePointCloud2)
00244   {
00245     m_cloudSub = nh.subscribe (m_stTopic, 3, &RangeSensor::CallBackPCL2, this);
00246   }
00247   else
00248   {
00249     m_cloudSub = nh.subscribe (m_stTopic, 3, &RangeSensor::CallBack, this);
00250   }
00251   m_grabbing = true;
00252   return true;
00253 }
00258 bool    RangeSensor::Stop()
00259 {
00260   Sensor::Stop();
00261   m_grabbing = false;
00262   return true;
00263   /*TODO unsunscribe*/
00264 }
00265 
00266 cop::XMLTag* RangeSensor::Save()
00267 {
00268   cop::XMLTag* tag = new cop::XMLTag(GetName());
00269   cop::Sensor::SaveTo(tag);
00270   tag->AddProperty(XML_PROPERTY_TOPIC, m_stTopic);
00271   tag->AddProperty(XML_PROPERTY_POINT_CLOUD2, m_subscribePointCloud2 ? 1 : 0);
00272   return tag;
00273 }


cop_sr4_plugins
Author(s): U. Klank
autogenerated on Mon Oct 6 2014 10:59:31