From c0ad31e9926b98ff2e83f85938001f70376afba1 Mon Sep 17 00:00:00 2001
From: Fumio KANEHIRO <fkanehiro@gmail.com>
Date: Sat, 23 Jan 2021 09:40:38 +0900
Subject: [PATCH] enable to compile with OpenRTM-1.2.2

---
 idl/HRPDataTypes.idl                                | 1 +
 idl/Img.idl                                         | 6 ++++++
 idl/PCDLoaderService.idl                            | 6 ++++++
 rtc/ImageData2CameraImage/ImageData2CameraImage.cpp | 8 +++++---
 rtc/ImageData2CameraImage/ImageData2CameraImage.h   | 5 +++--
 rtc/PCDLoader/PCDLoader.cpp                         | 8 ++++----
 rtc/PCDLoader/PCDLoader.h                           | 4 ++--
 7 files changed, 27 insertions(+), 11 deletions(-)

diff --git a/idl/HRPDataTypes.idl b/idl/HRPDataTypes.idl
index 9b0783319a5..d26351d5c9a 100644
--- a/idl/HRPDataTypes.idl
+++ b/idl/HRPDataTypes.idl
@@ -13,6 +13,7 @@ module OpenHRP
   struct SceneState
   {
     double time;
+    RTC::Time tm;
     RobotStateSeq states;
   };
 
diff --git a/idl/Img.idl b/idl/Img.idl
index d538008aeac..d8f03c77215 100644
--- a/idl/Img.idl
+++ b/idl/Img.idl
@@ -27,6 +27,12 @@ struct ImageData
   sequence<octet> raw_data;
 };
 
+struct TimedImageData
+{
+  RTC::Time tm;
+  ImageData data;
+};
+
 
 /* camera image */
 struct CameraIntrinsicParameter
diff --git a/idl/PCDLoaderService.idl b/idl/PCDLoaderService.idl
index b1efa1d69dc..dfc733dc13b 100644
--- a/idl/PCDLoaderService.idl
+++ b/idl/PCDLoaderService.idl
@@ -14,6 +14,12 @@ module OpenHRP
     };
     
     typedef sequence<PCDOffset> PCDOffsetSeq;
+
+    struct TimedPCDOffsetSeq
+    {
+        RTC::Time tm;
+        PCDOffsetSeq data;
+    };
     
     interface PCDLoaderService
     {
diff --git a/rtc/ImageData2CameraImage/ImageData2CameraImage.cpp b/rtc/ImageData2CameraImage/ImageData2CameraImage.cpp
index dcae02649d8..bccf6d627fc 100644
--- a/rtc/ImageData2CameraImage/ImageData2CameraImage.cpp
+++ b/rtc/ImageData2CameraImage/ImageData2CameraImage.cpp
@@ -32,8 +32,8 @@ static const char* imagedata2cameraimage_spec[] =
 ImageData2CameraImage::ImageData2CameraImage(RTC::Manager* manager)
   : RTC::DataFlowComponentBase(manager),
     // <rtc-template block="initializer">
-    m_dataIn("imageIn", m_data.data.image),
-    m_dataOut("imageOut", m_data),
+    m_dataIn("imageIn", m_tid),
+    m_dataOut("imageOut", m_tci),
     // </rtc-template>
     dummy(0)
 {
@@ -68,7 +68,7 @@ RTC::ReturnCode_t ImageData2CameraImage::onInitialize()
   // Set CORBA Service Ports
   
   // </rtc-template>
-  m_data.error_code = 0;
+  m_tci.error_code = 0;
 
   return RTC::RTC_OK;
 }
@@ -112,6 +112,8 @@ RTC::ReturnCode_t ImageData2CameraImage::onExecute(RTC::UniqueId ec_id)
 {
   if (m_dataIn.isNew()){
     m_dataIn.read();
+    m_tci.tm = m_tid.tm;
+    m_tci.data.image = m_tid.data;
     m_dataOut.write();
   }
   return RTC::RTC_OK;
diff --git a/rtc/ImageData2CameraImage/ImageData2CameraImage.h b/rtc/ImageData2CameraImage/ImageData2CameraImage.h
index 603fa057a68..754af389280 100644
--- a/rtc/ImageData2CameraImage/ImageData2CameraImage.h
+++ b/rtc/ImageData2CameraImage/ImageData2CameraImage.h
@@ -103,11 +103,12 @@ class ImageData2CameraImage
   
   // </rtc-template>
 
-  Img::TimedCameraImage m_data;
+  Img::TimedImageData m_tid;
+  Img::TimedCameraImage m_tci;
 
   // DataInPort declaration
   // <rtc-template block="inport_declare">
-  InPort<Img::ImageData> m_dataIn;
+  InPort<Img::TimedImageData> m_dataIn;
   
   // </rtc-template>
 
diff --git a/rtc/PCDLoader/PCDLoader.cpp b/rtc/PCDLoader/PCDLoader.cpp
index 116399e9688..056778fc48b 100644
--- a/rtc/PCDLoader/PCDLoader.cpp
+++ b/rtc/PCDLoader/PCDLoader.cpp
@@ -177,8 +177,8 @@ void PCDLoader::setCloudXYZRGB(PointCloudTypes::PointCloud& cloud, const pcl::Po
 void PCDLoader::updateOffsetToCloudXYZ(void)
 {
     pcl::PointCloud<pcl::PointXYZ>::Ptr clouds(new pcl::PointCloud<pcl::PointXYZ>);
-    for (unsigned int i=0; i<m_offset.length(); i++){
-        const OpenHRP::PCDOffset& offset = m_offset[i];
+    for (unsigned int i=0; i<m_offset.data.length(); i++){
+        const OpenHRP::PCDOffset& offset = m_offset.data[i];
         const std::string label(offset.label);
         if( m_clouds_xyz.find(label) != m_clouds_xyz.end() ){
             const hrp::Vector3 center(offset.center.x, offset.center.y, offset.center.z);
@@ -211,8 +211,8 @@ void PCDLoader::updateOffsetToCloudXYZ(void)
 void PCDLoader::updateOffsetToCloudXYZRGB(void)
 {
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr clouds(new pcl::PointCloud<pcl::PointXYZRGB>);
-    for (unsigned int i=0; i<m_offset.length(); i++){
-        const OpenHRP::PCDOffset& offset = m_offset[i];
+    for (unsigned int i=0; i<m_offset.data.length(); i++){
+        const OpenHRP::PCDOffset& offset = m_offset.data[i];
         const std::string label(offset.label);
         if( m_clouds_xyzrgb.find(label) != m_clouds_xyzrgb.end() ){
             const hrp::Vector3 center(offset.center.x, offset.center.y, offset.center.z);
diff --git a/rtc/PCDLoader/PCDLoader.h b/rtc/PCDLoader/PCDLoader.h
index 729199a92ff..7ec9864734e 100644
--- a/rtc/PCDLoader/PCDLoader.h
+++ b/rtc/PCDLoader/PCDLoader.h
@@ -110,13 +110,13 @@ class PCDLoader
   // </rtc-template>
     
   PointCloudTypes::PointCloud m_cloud;
-  OpenHRP::PCDOffsetSeq m_offset;
+  OpenHRP::TimedPCDOffsetSeq m_offset;
   RTC::TimedBoolean m_isOutput;
   
   // DataInPort declaration
   // <rtc-template block="inport_declare">
   
-  InPort<OpenHRP::PCDOffsetSeq> m_offsetIn;
+  InPort<OpenHRP::TimedPCDOffsetSeq> m_offsetIn;
   
   // </rtc-template>