[Fawkes Git] branch/jrothe/realsense: 2 revs updated. (0.5.0-3084-g35194eb)

Johannes Rothe johannes.rothe at rwth-aachen.de
Wed Jun 22 17:02:26 CEST 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, jrothe/realsense has been updated
        to  35194eb6ae5982d9218b5492f9fabe983ec299de (commit)
       via  0fed722609ab62b796543d1f6c618b89705e1e12 (commit)
      from  76e1f5d6f60aaff25d6821aa189c7a7d3c0e0a1d (commit)

http://git.fawkesrobotics.org/fawkes.git/jrothe/realsense

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- *Log* ---------------------------------------------------------------
commit 0fed722609ab62b796543d1f6c618b89705e1e12
Author:     Johannes Rothe <johannes.rothe at rwth-aachen.de>
AuthorDate: Tue Jun 21 17:21:13 2016 +0200
Commit:     Johannes Rothe <johannes.rothe at rwth-aachen.de>
CommitDate: Tue Jun 21 17:21:13 2016 +0200

    realsense: add config support

http://git.fawkesrobotics.org/fawkes.git/commit/0fed722
http://trac.fawkesrobotics.org/changeset/0fed722

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 35194eb6ae5982d9218b5492f9fabe983ec299de
Author:     Johannes Rothe <johannes.rothe at rwth-aachen.de>
AuthorDate: Tue Jun 21 17:22:12 2016 +0200
Commit:     Johannes Rothe <johannes.rothe at rwth-aachen.de>
CommitDate: Tue Jun 21 17:22:12 2016 +0200

    realsense: properly stop camera in finalize()

http://git.fawkesrobotics.org/fawkes.git/commit/35194eb
http://trac.fawkesrobotics.org/changeset/35194eb

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------
 cfg/conf.d/realsense.yaml                  |   11 +++++++++
 src/plugins/realsense/realsense_thread.cpp |   32 +++++++++++++++++++++++----
 src/plugins/realsense/realsense_thread.h   |    4 +++
 3 files changed, 42 insertions(+), 5 deletions(-)
 create mode 100644 cfg/conf.d/realsense.yaml


- *Diffs* -------------------------------------------------------------

- *commit* 0fed722609ab62b796543d1f6c618b89705e1e12 - - - - - - - - - -
Author:  Johannes Rothe <johannes.rothe at rwth-aachen.de>
Date:    Tue Jun 21 17:21:13 2016 +0200
Subject: realsense: add config support

 cfg/conf.d/realsense.yaml                  |   11 +++++++++++
 src/plugins/realsense/realsense_thread.cpp |   12 +++++++++---
 src/plugins/realsense/realsense_thread.h   |    2 ++
 3 files changed, 22 insertions(+), 3 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/realsense/realsense_thread.cpp b/src/plugins/realsense/realsense_thread.cpp
index 63d552e..b98a7c8 100644
--- a/src/plugins/realsense/realsense_thread.cpp
+++ b/src/plugins/realsense/realsense_thread.cpp
@@ -40,6 +40,11 @@ RealsenseThread::RealsenseThread()
 void
 RealsenseThread::init()
 {
+  //set config values
+  const std::string cfg_prefix = "/realsense/";
+  frame_id_ = config->get_string(cfg_prefix + "frame_id");
+  pcl_id_ = config->get_string(cfg_prefix + "pcl_id");
+
   rs_stream_type_ = RS_STREAM_DEPTH;
   connect_and_start_camera();
   camera_scale_ = rs_get_device_depth_scale(rs_device_, NULL);
@@ -47,13 +52,14 @@ RealsenseThread::init()
   rs_get_stream_intrinsics(rs_device_, rs_stream_type_, &z_intrinsic_, &rs_error_);
   logger->log_info(name(), "Height: %i, Width: %i", z_intrinsic_.height, z_intrinsic_.width);
 
+  //initalize pointcloud
   realsense_depth_refptr_ = new Cloud();
   realsense_depth_ = pcl_utils::cloudptr_from_refptr(realsense_depth_refptr_);
-  realsense_depth_->header.frame_id = "cam_conveyor";
+  realsense_depth_->header.frame_id = frame_id_;
   realsense_depth_->width = z_intrinsic_.width;
   realsense_depth_->height = z_intrinsic_.height;
-  pcl_manager->add_pointcloud("/camera/depth/points", realsense_depth_refptr_);
-  //initialize the pointcloud with points
+  pcl_manager->add_pointcloud(pcl_id_.c_str(), realsense_depth_refptr_);
+  //fill pointcloud with empty points
   for(int i = 0; i < z_intrinsic_.height; i++) {
     for(int j = 0; j < z_intrinsic_.width;  j++) {
       realsense_depth_->push_back(PointType(0, 0, 0));
diff --git a/src/plugins/realsense/realsense_thread.h b/src/plugins/realsense/realsense_thread.h
index 572f6b5..8acf2d9 100644
--- a/src/plugins/realsense/realsense_thread.h
+++ b/src/plugins/realsense/realsense_thread.h
@@ -85,6 +85,8 @@ class RealsenseThread
   rs_stream rs_stream_type_;
   int num_of_cameras_;
   float camera_scale_;
+  std::string frame_id_;
+  std::string pcl_id_;
 
 };
 

- *commit* 35194eb6ae5982d9218b5492f9fabe983ec299de - - - - - - - - - -
Author:  Johannes Rothe <johannes.rothe at rwth-aachen.de>
Date:    Tue Jun 21 17:22:12 2016 +0200
Subject: realsense: properly stop camera in finalize()

 src/plugins/realsense/realsense_thread.cpp |   20 ++++++++++++++++++--
 src/plugins/realsense/realsense_thread.h   |    2 ++
 2 files changed, 20 insertions(+), 2 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/realsense/realsense_thread.cpp b/src/plugins/realsense/realsense_thread.cpp
index b98a7c8..4126d71 100644
--- a/src/plugins/realsense/realsense_thread.cpp
+++ b/src/plugins/realsense/realsense_thread.cpp
@@ -98,8 +98,10 @@ void
 RealsenseThread::finalize()
 {
   realsense_depth_refptr_.reset();
-  //TODO delete point objects and cloud
-  //TODO Makefile
+  pcl_manager->remove_pointcloud(pcl_id_.c_str());
+  stop_camera();
+  //TODO Makefile & SwitchInterface
+  //TODO Documentation with librealsense
 }
 
 bool
@@ -121,6 +123,7 @@ RealsenseThread::connect_and_start_camera()
   logger->log_info(name(), "Stream format: %s",
                    rs_format_to_string(rs_get_stream_format(rs_device_, rs_stream_type_, &rs_error_)));
   log_error();
+  camera_started_ = true;
   return true;
 }
 
@@ -177,3 +180,16 @@ RealsenseThread::log_depths(const uint16_t * image)
   }
   logger->log_info(name(), "%s\n\n\n\n\n", out.c_str());
 }
+
+void
+RealsenseThread::stop_camera()
+{
+  if (camera_started_){
+    logger->log_info(name(), "Stopping realsense camera ...");
+    rs_stop_device(rs_device_, &rs_error_);
+    rs_delete_context(rs_context_, &rs_error_);
+    log_error();
+    logger->log_info(name(), "Realsense camera stopped!");
+    camera_started_ = false;
+  }
+}
diff --git a/src/plugins/realsense/realsense_thread.h b/src/plugins/realsense/realsense_thread.h
index 8acf2d9..5b5f999 100644
--- a/src/plugins/realsense/realsense_thread.h
+++ b/src/plugins/realsense/realsense_thread.h
@@ -64,6 +64,7 @@ class RealsenseThread
   void log_error();
   void log_depths(const uint16_t * image);
   void fill_pointcloud();
+  void stop_camera();
 
   /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
   protected: virtual void run() { Thread::run(); }
@@ -87,6 +88,7 @@ class RealsenseThread
   float camera_scale_;
   std::string frame_id_;
   std::string pcl_id_;
+  bool camera_started_ = false;
 
 };
 




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list