From 5110cfe9f7afce4c5efe57ae817b32383cc8c9fc Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent.lamiraux@laas.fr>
Date: Fri, 30 Sep 2022 14:36:04 +0200
Subject: [PATCH] [PointCloud] Add idl method to refresh octree in
 gepetto-viewer.

---
 idl/hpp/agimus_idl/point-cloud.idl |  2 ++
 include/hpp/agimus/point-cloud.hh  |  9 ++++----
 src/point-cloud.cc                 | 33 +++++++++++++++++++-----------
 3 files changed, 28 insertions(+), 16 deletions(-)

diff --git a/idl/hpp/agimus_idl/point-cloud.idl b/idl/hpp/agimus_idl/point-cloud.idl
index dad5dd6..233d3a5 100644
--- a/idl/hpp/agimus_idl/point-cloud.idl
+++ b/idl/hpp/agimus_idl/point-cloud.idl
@@ -86,6 +86,8 @@ module hpp {
       void setDistanceBounds(in double min, in double max) raises(Error);
       /// Set whether to display octree in gepetto-viewer
       void setDisplay(in boolean flag) raises(Error);
+      /// Refresh octree in gepetto-viewer
+      void refreshOctree() raises(Error);
     }; // interface PointCloud
   }; // module agimus_idl
 }; // module hpp
diff --git a/include/hpp/agimus/point-cloud.hh b/include/hpp/agimus/point-cloud.hh
index 08fd586..7927d1a 100644
--- a/include/hpp/agimus/point-cloud.hh
+++ b/include/hpp/agimus/point-cloud.hh
@@ -82,6 +82,8 @@ namespace hpp {
       void setDistanceBounds(value_type min, value_type max);
       /// Set whether to display octree in gepetto-viewer
       void setDisplay(bool flag);
+      /// Refresh octree in gepetto-viewer
+      void refreshOctree();
       /// Callback to the point cloud topic
       void pointCloudCb(const sensor_msgs::PointCloud2ConstPtr& data);
       /// Set three points belonging to the object plan, in the object frame
@@ -130,10 +132,8 @@ namespace hpp {
       /// Test if a point is in the wanted range
       bool filterPoint(uint32_t pointcloud_id, uint32_t row_id);
 
-      void attachOctreeToRobot
-      (const OcTreePtr_t& octree, const std::string& octreeFrame);
-      bool displayOctree(const OcTreePtr_t& octree,
-			 const std::string& octreeFrame);
+      void attachOctreeToRobot(const std::string& octreeFrame);
+      bool displayOctree(const std::string& octreeFrame);
       bool undisplayOctree(const std::string& octreeFrame);
       ProblemSolverPtr_t problemSolver_;
       bool waitingForData_;
@@ -142,6 +142,7 @@ namespace hpp {
 
       // Vector of the different point clouds measured
       std::vector<PointMatrix_t> pointsInSensorFrame_;
+      OcTreePtr_t octree_;
       PointMatrix_t pointsInLinkFrame_;
       value_type minDistance_, maxDistance_;
       bool display_;
diff --git a/src/point-cloud.cc b/src/point-cloud.cc
index 5d85e59..a4818c4 100644
--- a/src/point-cloud.cc
+++ b/src/point-cloud.cc
@@ -123,9 +123,8 @@ namespace hpp {
       movePointCloud(octreeFrame, sensorFrame, configuration);
 
       // build octree
-      hpp::fcl::OcTreePtr_t octree(hpp::fcl::makeOctree(pointsInLinkFrame_,
-							resolution));
-      attachOctreeToRobot(octree, octreeFrame);
+      octree_ = hpp::fcl::makeOctree(pointsInLinkFrame_, resolution);
+      attachOctreeToRobot(octreeFrame);
       return true;
     }
 
@@ -149,7 +148,7 @@ namespace hpp {
         // Remove point cloud from gepetto-gui.
         undisplayOctree(octreeFrame);
       }
-
+      octree_.reset();
     }
 
     void PointCloud::setDistanceBounds(value_type min, value_type max)
@@ -162,6 +161,17 @@ namespace hpp {
       display_ = flag;
     }
 
+    /// Refresh octree in gepetto-viewer
+    void PointCloud::refreshOctree()
+    {
+      if (octreeFrame_.empty()) return;
+      if (display_){
+        // Remove point cloud from gepetto-gui.
+        undisplayOctree(octreeFrame_);
+        displayOctree(octreeFrame_);
+      }
+    }
+
     void checkFields(const std::vector<sensor_msgs::PointField>& fields)
     {
       // Check that number of fields is at least 3
@@ -260,7 +270,8 @@ namespace hpp {
         }
       }
       ROS_INFO_STREAM("Used "  << iPoint << " out of "
-                      << data->height * data->width " to build point cloud.")
+                      << data->height * data->width
+                      << " to build point cloud.");
       pointsInSensorFrame_[pc_id].conservativeResize(iPoint, 3);
     }
 
@@ -313,8 +324,7 @@ namespace hpp {
       }
     }
 
-    void PointCloud::attachOctreeToRobot
-    (const OcTreePtr_t& octree, const std::string& octreeFrame)
+    void PointCloud::attachOctreeToRobot(const std::string& octreeFrame)
     {
       const DevicePtr_t& robot (problemSolver_->robot());
       const Frame& of(robot->getFrameByName(octreeFrame));
@@ -328,7 +338,7 @@ namespace hpp {
       }
       ::pinocchio::GeometryObject octreeGo
 	        (name,std::numeric_limits<FrameIndex>::max(), pinOctreeFrame.parent,
-	        octree, Transform3f::Identity());
+	        octree_, Transform3f::Identity());
       GeomIndex octreeGeomId(robot->geomModel().addGeometryObject(octreeGo));
       // Add collision pairs with all objects not attached to the octree joint.
       for (std::size_t geomId=0; geomId <
@@ -351,14 +361,13 @@ namespace hpp {
 	      problemSolver_->resetProblem();
       if (display_){
         // Display point cloud in gepetto-gui.
-        displayOctree(octree, octreeFrame);
+        displayOctree(octreeFrame);
       }
     }
 
 #ifdef CLIENT_TO_GEPETTO_VIEWER
 
-    bool PointCloud::displayOctree
-    (const OcTreePtr_t& octree, const std::string& octreeFrame)
+    bool PointCloud::displayOctree(const std::string& octreeFrame)
     {
       std::string prefix("robot/");
       // Connect to gepetto-gui without raising exception
@@ -366,7 +375,7 @@ namespace hpp {
       gepetto::corbaserver::GraphicalInterface_var gui
 	(gepetto::viewer::corba::gui());
       std::string nodeName(prefix + octreeFrame + std::string("/octree"));
-      octree->exportAsObjFile("/tmp/octree.obj");
+      octree_->exportAsObjFile("/tmp/octree.obj");
       try {
 	// If node already exists, remove it
 	if (gui->nodeExists(nodeName.c_str())){