Range2PointCloud.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <math.h>
11 #include <hrpUtil/Eigen3d.h>
12 #include "Range2PointCloud.h"
13 
14 // Module specification
15 // <rtc-template block="module_spec">
16 static const char* range2pointcloud_spec[] =
17  {
18  "implementation_id", "Range2PointCloud",
19  "type_name", "Range2PointCloud",
20  "description", "range2pointcloud component",
21  "version", HRPSYS_PACKAGE_VERSION,
22  "vendor", "AIST",
23  "category", "example",
24  "activity_type", "DataFlowComponent",
25  "max_instance", "10",
26  "language", "C++",
27  "lang_type", "compile",
28  // Configuration variables
29 
30  ""
31  };
32 // </rtc-template>
33 
35  : RTC::DataFlowComponentBase(manager),
36  // <rtc-template block="initializer">
37  m_rangeIn("range", m_range),
38  m_cloudOut("cloud", m_cloud),
39  // </rtc-template>
40  dummy(0)
41 {
42 }
43 
45 {
46 }
47 
48 
49 
50 RTC::ReturnCode_t Range2PointCloud::onInitialize()
51 {
52  std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
53  // <rtc-template block="bind_config">
54  // Bind variables and configuration variable
55 
56  // </rtc-template>
57 
58  // Registration: InPort/OutPort/Service
59  // <rtc-template block="registration">
60  // Set InPort buffers
61  addInPort("range", m_rangeIn);
62 
63  // Set OutPort buffer
64  addOutPort("cloud", m_cloudOut);
65 
66  // Set service provider to Ports
67 
68  // Set service consumers to Ports
69 
70  // Set CORBA Service Ports
71 
72  // </rtc-template>
73 
75 
76  m_cloud.height = 1;
77  m_cloud.type = "xyz";
78  m_cloud.fields.length(3);
79  m_cloud.fields[0].name = "x";
80  m_cloud.fields[0].offset = 0;
81  m_cloud.fields[0].data_type = PointCloudTypes::FLOAT32;
82  m_cloud.fields[0].count = 4;
83  m_cloud.fields[1].name = "y";
84  m_cloud.fields[1].offset = 4;
85  m_cloud.fields[1].data_type = PointCloudTypes::FLOAT32;
86  m_cloud.fields[1].count = 4;
87  m_cloud.fields[2].name = "z";
88  m_cloud.fields[2].offset = 8;
89  m_cloud.fields[2].data_type = PointCloudTypes::FLOAT32;
90  m_cloud.fields[2].count = 4;
91  m_cloud.is_bigendian = false;
92  m_cloud.point_step = 16;
93  m_cloud.is_dense = true;
94 
95  return RTC::RTC_OK;
96 }
97 
98 
99 
100 /*
101 RTC::ReturnCode_t Range2PointCloud::onFinalize()
102 {
103  return RTC::RTC_OK;
104 }
105 */
106 
107 /*
108 RTC::ReturnCode_t Range2PointCloud::onStartup(RTC::UniqueId ec_id)
109 {
110  return RTC::RTC_OK;
111 }
112 */
113 
114 /*
115 RTC::ReturnCode_t Range2PointCloud::onShutdown(RTC::UniqueId ec_id)
116 {
117  return RTC::RTC_OK;
118 }
119 */
120 
122 {
123  std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
124  return RTC::RTC_OK;
125 }
126 
128 {
129  std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
130  return RTC::RTC_OK;
131 }
132 
134 {
135  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
136  if (!m_rangeIn.isNew()) return RTC::RTC_OK;
137 
138  m_cloud.width = 0;
139  int npoint=0;
140  int nlines=0;
141  while (m_rangeIn.isNew()){
142  nlines++;
143  m_rangeIn.read();
144  m_cloud.width += m_range.ranges.length();
145  m_cloud.row_step = m_cloud.point_step*m_cloud.width;
146  m_cloud.data.length(m_cloud.row_step);// shrinked later
147  // range -> point cloud
148  float *ptr = (float *)m_cloud.data.get_buffer() + npoint*4;
149  Pose3D &pose = m_range.geometry.geometry.pose;
150  hrp::Vector3 relP, absP, sensorP(pose.position.x,
151  pose.position.y,
152  pose.position.z);
153  hrp::Matrix33 sensorR = hrp::rotFromRpy(pose.orientation.r,
154  pose.orientation.p,
155  pose.orientation.y);
156  for (unsigned int i=0; i<m_range.ranges.length(); i++){
157  double th = m_range.config.minAngle + i*m_range.config.angularRes;
158  double d = m_range.ranges[i];
159  if (d==0) continue;
160  relP << -d*sin(th), 0, -d*cos(th);
161  absP = sensorP + sensorR*relP;
162  ptr[0] = absP[0];
163  ptr[1] = absP[1];
164  ptr[2] = absP[2];
165  //std::cout << "(" << i << "," << ptr[2] << "," << d << ")" << std::endl;
166  ptr+=4;
167  npoint++;
168  }
169  }
170 #if 0
171  std::cout << "Range2PointCloud: processed " << nlines << " lines, "
172  << npoint << " points" << std::endl;
173 #endif
174  m_cloud.width = npoint;
175  m_cloud.data.length(npoint*m_cloud.point_step);
176  m_cloudOut.write();
177 
178  return RTC::RTC_OK;
179 }
180 
181 /*
182 RTC::ReturnCode_t Range2PointCloud::onAborting(RTC::UniqueId ec_id)
183 {
184  return RTC::RTC_OK;
185 }
186 */
187 
188 /*
189 RTC::ReturnCode_t Range2PointCloud::onError(RTC::UniqueId ec_id)
190 {
191  return RTC::RTC_OK;
192 }
193 */
194 
195 /*
196 RTC::ReturnCode_t Range2PointCloud::onReset(RTC::UniqueId ec_id)
197 {
198  return RTC::RTC_OK;
199 }
200 */
201 
202 /*
203 RTC::ReturnCode_t Range2PointCloud::onStateUpdate(RTC::UniqueId ec_id)
204 {
205  return RTC::RTC_OK;
206 }
207 */
208 
209 /*
210 RTC::ReturnCode_t Range2PointCloud::onRateChanged(RTC::UniqueId ec_id)
211 {
212  return RTC::RTC_OK;
213 }
214 */
215 
216 
217 
218 extern "C"
219 {
220 
222  {
224  manager->registerFactory(profile,
225  RTC::Create<Range2PointCloud>,
226  RTC::Delete<Range2PointCloud>);
227  }
228 
229 };
230 
231 
ComponentProfile m_profile
d
png_infop png_charpp int png_charpp profile
void Range2PointCloudInit(RTC::Manager *manager)
PointCloudTypes::PointCloud m_cloud
png_voidp ptr
png_uint_32 i
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
coil::Properties & getProperties()
OutPort< PointCloudTypes::PointCloud > m_cloudOut
bool addOutPort(const char *name, OutPortBase &outport)
Eigen::Vector3d Vector3
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
Matrix33 rotFromRpy(const Vector3 &rpy)
Eigen::Matrix3d Matrix33
range2pointcloud component
virtual ~Range2PointCloud()
Destructor.
static const char * range2pointcloud_spec[]
ExecutionContextHandle_t UniqueId
InPort< RangeData > m_rangeIn
prop
virtual bool isNew()
virtual bool write(DataType &value)
bool addInPort(const char *name, InPortBase &inport)
Range2PointCloud(RTC::Manager *manager)
Constructor.
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
virtual RTC::ReturnCode_t onInitialize()
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20