Sensor.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009 by Ulrich Friedrich Klank <klank@in.tum.de>
00003  *
00004  * This program is free software; you can redistribute it and/or modify
00005  * it under the terms of the GNU General Public License as published by
00006  * the Free Software Foundation; either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 
00019 /*********************************************************************
00020 *  Camera.cpp Copyright klank
00021 *********************************************************************/
00022 
00023 
00024 
00025 #include "XMLTag.h"
00026 #include "Sensor.h"
00027 
00028 #include "XMLTag.h"
00029 
00030 #include <pluginlib/class_loader.h>
00031 
00032 #ifdef BOOST_THREAD
00033 #include "boost/bind.hpp"
00034 #define BOOST(A) A
00035 #else
00036 #define BOOST(A)
00037 #endif
00038 
00039 
00040 using namespace cop;
00041 
00042 Sensor::~Sensor()
00043 {
00044   for(std::vector<Reading*>::iterator iter = m_images.begin(); iter != m_images.end(); iter++)
00045   {
00046     delete (*iter);
00047   }
00048   m_images.clear();
00049 }
00050 
00051 pluginlib::ClassLoader<Sensor> s_sens_loader("cognitive_perception", "Sensor");
00052 
00053 Sensor* Sensor::SensorFactory(XMLTag* tag)
00054 {
00055   Sensor* sens = NULL;
00056   try
00057   {
00058     sens = s_sens_loader.createClassInstance(tag->GetName());
00059     if(sens != NULL)
00060     {
00061       printf("Calling sens (%p)->SetData(%s)\n", sens, sens->GetName().c_str());
00062       sens->SetData(tag);
00063       if(sens->Start())
00064         printf("Sensor of type %s started\n", sens->GetName().c_str());
00065       else
00066         printf("Error: Sensor of type %s did NOT start\n", sens->GetName().c_str());
00067     }
00068     else
00069     {
00070       printf("A sensor was not loaded :(\n");
00071     }
00072   }
00073   catch(pluginlib::PluginlibException& ex)
00074   {
00075   //handle the class failing to load
00076     sens = NULL;
00077     printf("The plugin failed to load for some reason. Error: %s\n", ex.what());
00078     printf("Tag failed: %s\n", tag->GetName().c_str());
00079 
00080   }
00081         return sens;
00082 }
00083 
00084 void Sensor::WaitForNewData()
00085 {
00086  lock lk(m_mutexImageList);
00087  printf("Waiting for new data");
00088  m_newDataArrived.wait(lk);
00089  printf("Got new data\n");
00090 }
00091 
00092 
00093 bool Sensor::DeleteReading()
00094 {
00095   bool ret = false;
00096 //  static int count_occ = 0;
00097   lock lk(m_mutexImageList);
00098   try
00099   {
00100     if(m_images.size() == 0)
00101       return true;
00102     if((*m_images.begin())->m_usageCount == 0)
00103     {
00104       delete (*m_images.begin());
00105       m_images.erase(m_images.begin());
00106       m_deletedOffset++;
00107       ret = true;
00108     }
00109     else
00110     {
00111       m_temp_images.push_back(*m_images.begin());
00112       m_images.erase(m_images.begin());
00113       if((*m_images.begin())->m_usageCount == 0)
00114       {
00115         delete (*m_images.begin());
00116         m_images.erase(m_images.begin());
00117         m_deletedOffset++;
00118         ret = true;
00119 
00120       }
00121     }
00122   }
00123   catch(...)
00124   {
00125     printf("Error deleting Reading in Sensor::DeleteReading\n");
00126   }
00127   return ret;
00128 }
00129 
00130 
00131 void Sensor::SetData(XMLTag* tag)
00132 {
00133   printf("Entering set data\n");
00134   if(tag != NULL)
00135   {
00136     m_stSensorName = tag->GetProperty(XML_PROPERTY_SENSORNAME, "");
00137     XMLTag* pose_elem = tag->GetChild(XML_NODE_RELPOSE);
00138     if(pose_elem != NULL)
00139     {
00140       m_relPose = RelPoseFactory::FRelPose(pose_elem);
00141       if(m_relPose == NULL)
00142         printf("Error loading RelPose of Camera\n");
00143       else
00144         printf("Camera Located at %ld (%s)\n", m_relPose->m_uniqueID, m_relPose->m_mapstring.c_str());
00145     }
00146     else
00147     {
00148       m_relPose = RelPoseFactory::FRelPoseWorld();
00149     }
00150   }
00151 }
00152 
00153 
00154 void Sensor::SaveTo(XMLTag* tag)
00155 {
00156   if(m_relPose != NULL)
00157     tag->AddChild(m_relPose->Save());
00158 }
00159 
00160 Reading* Sensor::GetReading_Lock(size_t index)
00161 {
00162   Reading* ret = NULL;
00163   RelPose* temp = RelPoseFactory::GetRelPose(m_relPose->m_mapstring);
00164   if(temp!= NULL && temp->m_uniqueID != m_relPose->m_uniqueID)
00165   {
00166     ROS_ERROR("jlo tree was corrupted: id of sensor changed: %s (%ld <- %ld)", m_relPose->m_mapstring.c_str(), temp->m_uniqueID, m_relPose->m_uniqueID);
00167     RelPoseFactory::FreeRelPose(&m_relPose);
00168     m_relPose = temp;
00169   }
00170   else
00171   {
00172      RelPoseFactory::FreeRelPose(&temp);
00173   }
00174   lock lk(m_mutexImageList);
00175   if(index >= m_images.size())
00176   {
00177     printf("Camera skew asked %ldth frame which does not exist, will return %ld\n", index, m_images.size() - 1 );
00178     index = m_images.size() -1;
00179     if( m_images.size() == 0)
00180     {
00181       throw "Sensor broken\n";
00182     }
00183   }
00184   ret = ApplySelfFilter(m_images[index]);
00185   ret->Hold();
00186   return ret;
00187 }
00188 
00189 void Sensor::ProjectPoint3DToSensor(const double &x, const double &y, const double &z, double &row, double &column) const
00190 {
00191   MinimalCalibration(GetUnformatedCalibrationValues()).Project3DPoint(x,y,z,row, column) ;
00192 }
00193 
00194 #include <ros/ros.h>
00195 #include <sensor_msgs/PointCloud.h>
00196 
00197 bool s_initedPCDPublisher = false;
00198 ros::Publisher s_sensorPublisher;
00199 
00200 
00201 void Sensor::Publish3DData(const std::vector<double> &x, const std::vector<double> &y, const std::vector<double> &z)
00202 {
00203   if(!s_initedPCDPublisher)
00204   {
00205     s_initedPCDPublisher = true;
00206     ros::NodeHandle nh;
00207     printf("\n\n\nAdvertise new topic for Displaying results: /cop/pcds\n\n\n\n");
00208     s_sensorPublisher = nh.advertise<sensor_msgs::PointCloud>("/cop/pcds", 2, true);
00209   }
00210   sensor_msgs::PointCloud pcd;
00211   pcd.header.frame_id = "/map";
00212   pcd.header.stamp = ros::Time::now();
00213 
00214   for(size_t i = 0; i < x.size(); i++)
00215   {
00216     geometry_msgs::Point32 point;
00217     point.x = x[i];
00218     point.y = y[i];
00219     point.z = z[i];
00220     pcd.points.push_back(point);
00221   }
00222   printf("Call Publish: /cop/pcds\n");
00223   s_sensorPublisher.publish(pcd);
00224 }
00225 
00226 
00227 void Sensor::PushBack(Reading* img)
00228 {
00229   lock lk(m_mutexImageList);
00230   m_images.push_back(img);
00231   img->SetPose(m_relPose);
00232   m_FrameCount++;
00233   m_newDataArrived.notify_all();
00234 }
00235 
00236 MinimalCalibration::MinimalCalibration(XMLTag* tag)
00237 {
00238   focal_length = tag->GetPropertyDouble("MinimalCalibration_focal_length", 1);
00239   pix_size_x = tag->GetPropertyDouble("MinimalCalibration_pix_size_x", 1);
00240   pix_size_y = tag->GetPropertyDouble("MinimalCalibration_pix_size_y", 1);
00241   proj_center_x = tag->GetPropertyDouble("MinimalCalibration_proj_center_x", 0.5);
00242   proj_center_y = tag->GetPropertyDouble("MinimalCalibration_proj_center_y", 0.5);
00243   width = tag->GetPropertyDouble("MinimalCalibration_width", 0.5);
00244   height = tag->GetPropertyDouble("MinimalCalibration_height", 0.5);
00245 }
00246 void MinimalCalibration::SetData(XMLTag* tag)
00247 {
00248   tag->AddProperty("MinimalCalibration_focal_length", focal_length);
00249   tag->AddProperty("MinimalCalibration_pix_size_x", pix_size_x);
00250   tag->AddProperty("MinimalCalibration_pix_size_y", pix_size_y);
00251   tag->AddProperty("MinimalCalibration_proj_center_x", proj_center_x);
00252   tag->AddProperty("MinimalCalibration_proj_center_y", proj_center_y);
00253   tag->AddProperty("MinimalCalibration_width", width);
00254   tag->AddProperty("MinimalCalibration_height",height);
00255 }
00256 
00257 
00258 #ifdef USE_YARP_COMM
00259 /*
00260 template <class SensorType, class MessageType>
00261 void SensorNetworkRelay<SensorType, MessageType>::SetData(XMLTag* tag)
00262 {
00263   SensorType::SetData(tag);
00264   m_stTopic = tag->GetProperty(XML_ATTRIBUTE_TOPICNAME);
00265   m_rate = tag->GetPropertyInt(XML_ATTRIBUTE_RATE);
00266   ros::NodeHandle nh;
00267   m_pub = nh.advertise<MessageType>(m_stTopic, 5);
00268 }
00269 */
00283 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


cognitive_perception
Author(s): Ulrich F Klank
autogenerated on Thu May 23 2013 07:38:35