Create action server and data for rectification only once, delete all…

…ocated data conditionally
magazino · Jan 25, 2018 · 559d969 · 559d969
1 parent 8eeee9e
commit 559d969
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 17 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -91,7 +91,6 @@ target_link_libraries(
 add_dependencies(
     ${PROJECT_NAME}
      ${catkin_EXPORTED_TARGETS}
-     camera_control_msgs
 )
 
 # Add pylon_camera_node

diff --git a/src/pylon_camera/pylon_camera_node.cpp b/src/pylon_camera/pylon_camera_node.cpp
@@ -348,23 +348,35 @@ bool PylonCameraNode::startGrabbing()
 
 void PylonCameraNode::setupRectification()
 {
-    img_rect_pub_ =
-        new ros::Publisher(nh_.advertise<sensor_msgs::Image>("image_rect", 1));
-
-    grab_imgs_rect_as_ =
-        new GrabImagesAS(nh_,
-                         "grab_images_rect",
-                         boost::bind(
-                            &PylonCameraNode::grabImagesRectActionExecuteCB,
-                            this,
-                            _1),
-                         false);
-
-    pinhole_model_ = new image_geometry::PinholeCameraModel();
-    pinhole_model_->fromCameraInfo(camera_info_manager_->getCameraInfo());
-    grab_imgs_rect_as_->start();
+    if (!img_rect_pub_)
+    {
+        img_rect_pub_ =
+            new ros::Publisher(nh_.advertise<sensor_msgs::Image>("image_rect", 1));
+    }
+
+    if (!grab_imgs_rect_as_)
+    {
+        grab_imgs_rect_as_ =
+            new GrabImagesAS(nh_,
+                             "grab_images_rect",
+                             boost::bind(
+                                &PylonCameraNode::grabImagesRectActionExecuteCB,
+                                this,
+                                _1),
+                             false);
+        grab_imgs_rect_as_->start();
+    }
+
+    if (!pinhole_model_)
+    {
+        pinhole_model_ = new image_geometry::PinholeCameraModel();
+    }
 
-    cv_bridge_img_rect_ = new cv_bridge::CvImage();
+    pinhole_model_->fromCameraInfo(camera_info_manager_->getCameraInfo());
+    if (!cv_bridge_img_rect_)
+    {
+        cv_bridge_img_rect_ = new cv_bridge::CvImage();
+    }
     cv_bridge_img_rect_->header = img_raw_msg_.header;
     cv_bridge_img_rect_->encoding = img_raw_msg_.encoding;
 }
@@ -1532,6 +1544,20 @@ PylonCameraNode::~PylonCameraNode()
         delete it_;
         it_ = nullptr;
     }
+    if (grab_imgs_rect_as_)
+    {
+        grab_imgs_rect_as_->shutdown();
+        delete grab_imgs_rect_as_;
+    }
+
+    if (img_rect_pub_)
+        delete img_rect_pub_;
+
+    if (cv_bridge_img_rect_)
+        delete cv_bridge_img_rect_;
+
+    if (pinhole_model_)
+        delete pinhole_model_;
 }
 
 }  // namespace pylon_camera