From 317b66eff963e83793025b4f0020fba10479a635 Mon Sep 17 00:00:00 2001
From: Fumio KANEHIRO <fkanehiro@gmail.com>
Date: Sat, 14 Feb 2015 23:43:55 +0900
Subject: [PATCH] adds an inport to receive range data

---
 rtc/OccupancyGridMap3D/OccupancyGridMap3D.cpp | 23 +++++++++++++++++++
 rtc/OccupancyGridMap3D/OccupancyGridMap3D.h   |  2 ++
 2 files changed, 25 insertions(+)

diff --git a/rtc/OccupancyGridMap3D/OccupancyGridMap3D.cpp b/rtc/OccupancyGridMap3D/OccupancyGridMap3D.cpp
index 3a93ee33505..b56c9455fde 100644
--- a/rtc/OccupancyGridMap3D/OccupancyGridMap3D.cpp
+++ b/rtc/OccupancyGridMap3D/OccupancyGridMap3D.cpp
@@ -52,6 +52,7 @@ static const char* occupancygridmap3d_spec[] =
 OccupancyGridMap3D::OccupancyGridMap3D(RTC::Manager* manager)
   : RTC::DataFlowComponentBase(manager),
     // <rtc-template block="initializer">
+    m_rangeIn("range", m_range),
     m_cloudIn("cloud", m_cloud),
     m_poseIn("pose", m_pose),
     m_updateIn("update", m_update),
@@ -87,6 +88,7 @@ RTC::ReturnCode_t OccupancyGridMap3D::onInitialize()
   // Registration: InPort/OutPort/Service
   // <rtc-template block="registration">
   // Set InPort buffers
+  addInPort("range", m_rangeIn);
   addInPort("cloud", m_cloudIn);
   addInPort("pose", m_poseIn);
   addInPort("update", m_updateIn);
@@ -209,6 +211,27 @@ RTC::ReturnCode_t OccupancyGridMap3D::onExecute(RTC::UniqueId ec_id)
         return RTC::RTC_OK;
     }
 
+    while (m_rangeIn.isNew()){
+        m_rangeIn.read();
+        Guard guard(m_mutex);
+        Pointcloud cloud;
+        for (unsigned int i=0; i<m_range.ranges.length(); i++){
+            double th = m_range.config.minAngle + i*m_range.config.angularRes;
+            double d = m_range.ranges[i];
+            if (d==0) continue;
+            cloud.push_back(point3d(-d*sin(th), 0, -d*cos(th)));
+        }
+        point3d sensor(0,0,0);
+        Pose3D &pose = m_range.geometry.geometry.pose;
+        pose6d frame(pose.position.x,
+                     pose.position.y,
+                     pose.position.z, 
+                     pose.orientation.r,
+                     pose.orientation.p,
+                     pose.orientation.y);
+        m_map->insertPointCloud(cloud, sensor, frame);
+    }
+
     if (m_cloudIn.isNew()){
         while (m_cloudIn.isNew()) m_cloudIn.read();
         while (m_poseIn.isNew())  m_poseIn.read();
diff --git a/rtc/OccupancyGridMap3D/OccupancyGridMap3D.h b/rtc/OccupancyGridMap3D/OccupancyGridMap3D.h
index 432eff6a989..6c6407a057d 100644
--- a/rtc/OccupancyGridMap3D/OccupancyGridMap3D.h
+++ b/rtc/OccupancyGridMap3D/OccupancyGridMap3D.h
@@ -115,9 +115,11 @@ class OccupancyGridMap3D
   PointCloudTypes::PointCloud m_cloud;
   TimedPose3D m_pose;  
   TimedLong m_update;
+  RangeData m_range;  
 
   // DataInPort declaration
   // <rtc-template block="inport_declare">
+  InPort<RangeData> m_rangeIn;
   InPort<PointCloudTypes::PointCloud> m_cloudIn;
   InPort<TimedPose3D> m_poseIn;
   InPort<TimedLong> m_updateIn;