Range2PointCloud.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <math.h>
00011 #include <hrpUtil/Eigen3d.h>
00012 #include "Range2PointCloud.h"
00013 
00014 // Module specification
00015 // <rtc-template block="module_spec">
00016 static const char* range2pointcloud_spec[] =
00017   {
00018     "implementation_id", "Range2PointCloud",
00019     "type_name",         "Range2PointCloud",
00020     "description",       "range2pointcloud component",
00021     "version",           HRPSYS_PACKAGE_VERSION,
00022     "vendor",            "AIST",
00023     "category",          "example",
00024     "activity_type",     "DataFlowComponent",
00025     "max_instance",      "10",
00026     "language",          "C++",
00027     "lang_type",         "compile",
00028     // Configuration variables
00029 
00030     ""
00031   };
00032 // </rtc-template>
00033 
00034 Range2PointCloud::Range2PointCloud(RTC::Manager* manager)
00035   : RTC::DataFlowComponentBase(manager),
00036     // <rtc-template block="initializer">
00037     m_rangeIn("range", m_range),
00038     m_cloudOut("cloud", m_cloud),
00039     // </rtc-template>
00040     dummy(0)
00041 {
00042 }
00043 
00044 Range2PointCloud::~Range2PointCloud()
00045 {
00046 }
00047 
00048 
00049 
00050 RTC::ReturnCode_t Range2PointCloud::onInitialize()
00051 {
00052   std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00053   // <rtc-template block="bind_config">
00054   // Bind variables and configuration variable
00055   
00056   // </rtc-template>
00057 
00058   // Registration: InPort/OutPort/Service
00059   // <rtc-template block="registration">
00060   // Set InPort buffers
00061   addInPort("range", m_rangeIn);
00062 
00063   // Set OutPort buffer
00064   addOutPort("cloud", m_cloudOut);
00065   
00066   // Set service provider to Ports
00067   
00068   // Set service consumers to Ports
00069   
00070   // Set CORBA Service Ports
00071   
00072   // </rtc-template>
00073 
00074   RTC::Properties& prop = getProperties();
00075 
00076   m_cloud.height = 1;
00077   m_cloud.type = "xyz";
00078   m_cloud.fields.length(3);
00079   m_cloud.fields[0].name = "x";
00080   m_cloud.fields[0].offset = 0;
00081   m_cloud.fields[0].data_type = PointCloudTypes::FLOAT32;
00082   m_cloud.fields[0].count = 4;
00083   m_cloud.fields[1].name = "y";
00084   m_cloud.fields[1].offset = 4;
00085   m_cloud.fields[1].data_type = PointCloudTypes::FLOAT32;
00086   m_cloud.fields[1].count = 4;
00087   m_cloud.fields[2].name = "z";
00088   m_cloud.fields[2].offset = 8;
00089   m_cloud.fields[2].data_type = PointCloudTypes::FLOAT32;
00090   m_cloud.fields[2].count = 4;
00091   m_cloud.is_bigendian = false;
00092   m_cloud.point_step = 16;
00093   m_cloud.is_dense = true;
00094 
00095   return RTC::RTC_OK;
00096 }
00097 
00098 
00099 
00100 /*
00101 RTC::ReturnCode_t Range2PointCloud::onFinalize()
00102 {
00103   return RTC::RTC_OK;
00104 }
00105 */
00106 
00107 /*
00108 RTC::ReturnCode_t Range2PointCloud::onStartup(RTC::UniqueId ec_id)
00109 {
00110   return RTC::RTC_OK;
00111 }
00112 */
00113 
00114 /*
00115 RTC::ReturnCode_t Range2PointCloud::onShutdown(RTC::UniqueId ec_id)
00116 {
00117   return RTC::RTC_OK;
00118 }
00119 */
00120 
00121 RTC::ReturnCode_t Range2PointCloud::onActivated(RTC::UniqueId ec_id)
00122 {
00123   std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
00124   return RTC::RTC_OK;
00125 }
00126 
00127 RTC::ReturnCode_t Range2PointCloud::onDeactivated(RTC::UniqueId ec_id)
00128 {
00129   std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
00130   return RTC::RTC_OK;
00131 }
00132 
00133 RTC::ReturnCode_t Range2PointCloud::onExecute(RTC::UniqueId ec_id)
00134 {
00135     //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
00136   if (!m_rangeIn.isNew()) return RTC::RTC_OK;
00137 
00138   m_cloud.width = 0;
00139   int npoint=0;
00140   int nlines=0;
00141   while (m_rangeIn.isNew()){
00142     nlines++;
00143     m_rangeIn.read();
00144     m_cloud.width += m_range.ranges.length();
00145     m_cloud.row_step = m_cloud.point_step*m_cloud.width;
00146     m_cloud.data.length(m_cloud.row_step);// shrinked later
00147     // range -> point cloud
00148     float *ptr = (float *)m_cloud.data.get_buffer() + npoint*4;
00149     Pose3D &pose = m_range.geometry.geometry.pose;
00150     hrp::Vector3 relP, absP, sensorP(pose.position.x,
00151                                      pose.position.y,
00152                                      pose.position.z);
00153     hrp::Matrix33 sensorR = hrp::rotFromRpy(pose.orientation.r,
00154                                             pose.orientation.p,
00155                                             pose.orientation.y);
00156     for (unsigned int i=0; i<m_range.ranges.length(); i++){
00157       double th = m_range.config.minAngle + i*m_range.config.angularRes;
00158       double d = m_range.ranges[i];
00159       if (d==0) continue;
00160       relP << -d*sin(th), 0, -d*cos(th);
00161       absP = sensorP + sensorR*relP;
00162       ptr[0] = absP[0];
00163       ptr[1] = absP[1];
00164       ptr[2] = absP[2];
00165       //std::cout << "(" << i << "," << ptr[2] << "," << d << ")" << std::endl;
00166       ptr+=4;
00167       npoint++;
00168     }
00169   }
00170 #if 0
00171   std::cout << "Range2PointCloud: processed " << nlines << " lines, " 
00172             << npoint << " points" << std::endl;
00173 #endif
00174   m_cloud.width = npoint;
00175   m_cloud.data.length(npoint*m_cloud.point_step);
00176   m_cloudOut.write();
00177 
00178   return RTC::RTC_OK;
00179 }
00180 
00181 /*
00182 RTC::ReturnCode_t Range2PointCloud::onAborting(RTC::UniqueId ec_id)
00183 {
00184   return RTC::RTC_OK;
00185 }
00186 */
00187 
00188 /*
00189 RTC::ReturnCode_t Range2PointCloud::onError(RTC::UniqueId ec_id)
00190 {
00191   return RTC::RTC_OK;
00192 }
00193 */
00194 
00195 /*
00196 RTC::ReturnCode_t Range2PointCloud::onReset(RTC::UniqueId ec_id)
00197 {
00198   return RTC::RTC_OK;
00199 }
00200 */
00201 
00202 /*
00203 RTC::ReturnCode_t Range2PointCloud::onStateUpdate(RTC::UniqueId ec_id)
00204 {
00205   return RTC::RTC_OK;
00206 }
00207 */
00208 
00209 /*
00210 RTC::ReturnCode_t Range2PointCloud::onRateChanged(RTC::UniqueId ec_id)
00211 {
00212   return RTC::RTC_OK;
00213 }
00214 */
00215 
00216 
00217 
00218 extern "C"
00219 {
00220 
00221   void Range2PointCloudInit(RTC::Manager* manager)
00222   {
00223     RTC::Properties profile(range2pointcloud_spec);
00224     manager->registerFactory(profile,
00225                              RTC::Create<Range2PointCloud>,
00226                              RTC::Delete<Range2PointCloud>);
00227   }
00228 
00229 };
00230 
00231 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55