Fixed activate and deactivate to avoid errors

GaitechYu · May 4, 2016 · a25c76b · a25c76b
1 parent e6d6b59
commit a25c76b
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 25 deletions.
diff --git a/fetch_depth_layer/include/fetch_depth_layer/depth_layer.h b/fetch_depth_layer/include/fetch_depth_layer/depth_layer.h
@@ -66,16 +66,6 @@ class FetchDepthLayer : public VoxelLayer
    */
   virtual void onInitialize();
 
-  /**
-   * @brief Reinitializes class
-   */
-  virtual void activate();
-
-  /**
-   * @brief Unsubscribes from topics
-   */
-  virtual void deactivate();
-
 private:
   void cameraInfoCallback(
     const sensor_msgs::CameraInfo::ConstPtr& msg);
@@ -113,7 +103,7 @@ class FetchDepthLayer : public VoxelLayer
 
   // retrieves depth image from head_camera
   // used to fit ground plane to
-  message_filters::Subscriber<sensor_msgs::Image> depth_image_sub_;
+  boost::shared_ptr< message_filters::Subscriber<sensor_msgs::Image> > depth_image_sub_;
   boost::shared_ptr< tf::MessageFilter<sensor_msgs::Image> > depth_image_filter_;
 
   // retrieves camera matrix for head_camera

diff --git a/fetch_depth_layer/src/depth_layer.cpp b/fetch_depth_layer/src/depth_layer.cpp
@@ -90,6 +90,7 @@ void FetchDepthLayer::onInitialize()
   	  obstacle_range, raytrace_range, *tf_, global_frame_,
   	  sensor_frame, transform_tolerance));
   marking_buffers_.push_back(marking_buf_);
+  observation_buffers_.push_back(marking_buf_);
 
   min_obstacle_height = 0.0;
 
@@ -99,6 +100,7 @@ void FetchDepthLayer::onInitialize()
   	  obstacle_range, raytrace_range, *tf_, global_frame_,
   	  sensor_frame, transform_tolerance));
   clearing_buffers_.push_back(clearing_buf_);
+  observation_buffers_.push_back(clearing_buf_);
 
   if (publish_observations_)
   {
@@ -115,10 +117,13 @@ void FetchDepthLayer::onInitialize()
   camera_info_sub_ = private_nh.subscribe<sensor_msgs::CameraInfo>(
     camera_info_topic, 10, &FetchDepthLayer::cameraInfoCallback, this);
 
-  depth_image_sub_.subscribe(private_nh, camera_depth_topic, 10);
+  depth_image_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(private_nh, camera_depth_topic, 10));
   depth_image_filter_ = boost::shared_ptr< tf::MessageFilter<sensor_msgs::Image> >(
-    new tf::MessageFilter<sensor_msgs::Image>(depth_image_sub_, *tf_, global_frame_, 10));
+    new tf::MessageFilter<sensor_msgs::Image>(*depth_image_sub_, *tf_, global_frame_, 10));
   depth_image_filter_->registerCallback(boost::bind(&FetchDepthLayer::depthImageCallback, this, _1));
+  observation_subscribers_.push_back(depth_image_sub_);
+  observation_notifiers_.push_back(depth_image_filter_);
+  observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
 }
 
 FetchDepthLayer::~FetchDepthLayer()
@@ -398,16 +403,4 @@ void FetchDepthLayer::depthImageCallback(
   }
 }
 
-void FetchDepthLayer::activate()
-{
-  onInitialize();
-}
-
-void FetchDepthLayer::deactivate()
-{
-  camera_info_sub_.shutdown();
-  depth_image_sub_.unsubscribe();
-  depth_image_filter_->clear();
-}
-
 }  // namespace costmap_2d