adds an inport to receive sensor position

fkanehiro · Feb 14, 2015 · a00f22e · a00f22e
1 parent 317b66e
commit a00f22e
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/rtc/OccupancyGridMap3D/OccupancyGridMap3D.cpp b/rtc/OccupancyGridMap3D/OccupancyGridMap3D.cpp
@@ -55,6 +55,7 @@ OccupancyGridMap3D::OccupancyGridMap3D(RTC::Manager* manager)
  m_rangeIn("range", m_range),
  m_cloudIn("cloud", m_cloud),
  m_poseIn("pose", m_pose),
+ m_sensorPosIn("sensorPos", m_sensorPos),
  m_updateIn("update", m_update),
  m_updateOut("updateSignal", m_updateSignal),
  m_OGMap3DServicePort("OGMap3DService"),
@@ -91,6 +92,7 @@ RTC::ReturnCode_t OccupancyGridMap3D::onInitialize()
  addInPort("range", m_rangeIn);
  addInPort("cloud", m_cloudIn);
  addInPort("pose", m_poseIn);
+ addInPort("sensorPos", m_sensorPosIn);
  addInPort("update", m_updateIn);
 
  // Set OutPort buffer
@@ -122,6 +124,10 @@ RTC::ReturnCode_t OccupancyGridMap3D::onInitialize()
  m_pose.data.orientation.p = 0;
  m_pose.data.orientation.y = 0;
 
+ m_sensorPos.data.x = 0;
+ m_sensorPos.data.y = 0;
+ m_sensorPos.data.z = 0;
+
  m_updateSignal.data = 0;
 
  return RTC::RTC_OK;
@@ -235,6 +241,7 @@ RTC::ReturnCode_t OccupancyGridMap3D::onExecute(RTC::UniqueId ec_id)
  if (m_cloudIn.isNew()){
  while (m_cloudIn.isNew()) m_cloudIn.read();
  while (m_poseIn.isNew()) m_poseIn.read();
+ while (m_sensorPosIn.isNew()) m_sensorPosIn.read();
  Guard guard(m_mutex);
  float *ptr = (float *)m_cloud.data.get_buffer();
  if (strcmp(m_cloud.type, "xyz")==0 
@@ -244,7 +251,9 @@ RTC::ReturnCode_t OccupancyGridMap3D::onExecute(RTC::UniqueId ec_id)
  if (isnan(ptr[0])) continue;
  cloud.push_back(point3d(ptr[0],ptr[1],ptr[2]));
  }
- point3d sensor(0,0,0);
+ point3d sensor(m_sensorPos.data.x,
+ m_sensorPos.data.y,
+ m_sensorPos.data.z);
  pose6d frame(m_pose.data.position.x,
  m_pose.data.position.y,
  m_pose.data.position.z, 

diff --git a/rtc/OccupancyGridMap3D/OccupancyGridMap3D.h b/rtc/OccupancyGridMap3D/OccupancyGridMap3D.h
@@ -113,7 +113,8 @@ class OccupancyGridMap3D
  // </rtc-template>
 
  PointCloudTypes::PointCloud m_cloud;
- TimedPose3D m_pose; 
+ TimedPose3D m_pose;
+ TimedPoint3D m_sensorPos;
  TimedLong m_update;
  RangeData m_range; 
 
@@ -122,6 +123,7 @@ class OccupancyGridMap3D
  InPort<RangeData> m_rangeIn;
  InPort<PointCloudTypes::PointCloud> m_cloudIn;
  InPort<TimedPose3D> m_poseIn;
+ InPort<TimedPoint3D> m_sensorPosIn;
  InPort<TimedLong> m_updateIn;
 
  // </rtc-template>