Merge pull request #27 from florent-lamiraux/devel

Fix display of octree in gepetto-gui

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

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_;

src/agimus_hpp/trajectory_publisher.py

@@ -254,7 +254,14 @@ def publish(self, empty):
         while n < len(self.times):
             if n < nstar:
                 prev = rospy.Time.now()
-                self.discretization.compute (self.times[n])
+                try:
+                    # If evaluation of the path for this parameter fails,
+                    # do not publish anything
+                    self.discretization.compute (self.times[n])
+                except:
+                    self.discretization.compute (self.times[n-1])
+                    rospy.logwarn("Failed to evaluate path at parameter {}".
+                                  format(self.times[n]))
                 now = rospy.Time.now()
                 computation_time += now - prev
                 n += 1

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
@@ -259,6 +269,9 @@ namespace hpp {
           ptr+=data->point_step;
         }
       }
+      ROS_INFO_STREAM("Used "  << iPoint << " out of "
+                      << data->height * data->width
+                      << " to build point cloud.");
       pointsInSensorFrame_[pc_id].conservativeResize(iPoint, 3);
     }
 
@@ -311,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));
@@ -326,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 <
@@ -349,22 +361,22 @@ namespace hpp {
 	      problemSolver_->resetProblem();
       if (display_){
         // Display point cloud in gepetto-gui.
-        displayOctree(octree, octreeFrame);
+        undisplayOctree(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
       gepetto::viewer::corba::connect(0x0, true);
       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())){
@@ -394,6 +406,7 @@ namespace hpp {
       try {
 	// If node already exists, remove it
 	if (gui->nodeExists(nodeName.c_str())){
+	  gui->removeObjectFromCache(nodeName.c_str());
 	  gui->deleteNode(nodeName.c_str(), true);
           return true;
 	}