[Fawkes Git] branch/common/caro-current: 3 revs updated. (0.5.0-3175-g5d86cb1)

Tobias Neumann tobias.neumann at alumni.fh-aachen.de
Tue Jun 21 19:40:18 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, common/caro-current has been updated
        to  5d86cb166eb4f0a3c81fd95cb4ca15bd8f430897 (commit)
       via  af30db550474cead809af16b02ec6093f4b0c4a8 (commit)
       via  0516b09939734306a619d6cd879ec7b91416ff45 (commit)
      from  2b5441c23331af3ab626bf1f707216853815708e (commit)

http://git.fawkesrobotics.org/fawkes.git/common/caro-current

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 0516b09939734306a619d6cd879ec7b91416ff45
Author:     Tobias Neumann <t.neumann at fh-aachen.de>
AuthorDate: Tue Apr 12 14:42:56 2016 +0200
Commit:     Tobias Neumann <t.neumann at fh-aachen.de>
CommitDate: Tue Jun 21 19:40:12 2016 +0200

    ros-pcl: add bidirectional exchange of pointclouds
    
    the ros-pcl plugin just copied pcs from fawkes->ros
    now it also sends pcs from ros-fawkes
    
    For this a polling (every 10 sec) is done on the ros topics to check for
    available topics. If the first pointcloud from ROS is received, this topic will be
    available in fawkes, one cloud needs to be received since otherwise the
    Point-Type used in this topic is unknown and a known type is a requirement for the
    pcl_manager. The supported Point-Types are:
    - XYZ
    - XYZRGB (untested)
    - XYZI (untested)
    Furthermore only topics that are used in fawkes are subscribed in ros,
    for this also a polling is done, which results in an offset up to 10 sec
    untill the first cloud is received in fawkes.

http://git.fawkesrobotics.org/fawkes.git/commit/0516b09
http://trac.fawkesrobotics.org/changeset/0516b09

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit af30db550474cead809af16b02ec6093f4b0c4a8
Author:     Tobias Neumann <t.neumann at fh-aachen.de>
AuthorDate: Tue Apr 12 15:22:42 2016 +0200
Commit:     Tobias Neumann <t.neumann at fh-aachen.de>
CommitDate: Tue Jun 21 19:40:12 2016 +0200

    ros-pcl: fixed segmentation fault (workaround?)
    
    if a pointcloud (fawkes->ros) is used in ros and the sender in fawkes is
    closed, the adapter in ros-pcl was closed, but the adapter was allready
    deleted during the closing in the sender.
    
    The best solution would be a fix in the adapter class, however it seems
    to look like a check is implemented, I couldn't find the reason why this
    does not work.

http://git.fawkesrobotics.org/fawkes.git/commit/af30db5
http://trac.fawkesrobotics.org/changeset/af30db5

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 5d86cb166eb4f0a3c81fd95cb4ca15bd8f430897
Author:     Tobias Neumann <t.neumann at fh-aachen.de>
AuthorDate: Mon May 9 17:11:57 2016 +0200
Commit:     Tobias Neumann <t.neumann at fh-aachen.de>
CommitDate: Tue Jun 21 19:40:12 2016 +0200

    ros-pcl: add changed requested by Tim
    
    - research interval for ROS pointclouds as a config
    - "_" at the end of private vars
    - const and & at foreach loops for speedup
    - more expressive log msgs

http://git.fawkesrobotics.org/fawkes.git/commit/5d86cb1
http://trac.fawkesrobotics.org/changeset/5d86cb1

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


- *Summary* -----------------------------------------------------------
 cfg/conf.d/ros.yaml            |    4 +
 src/plugins/ros/pcl_thread.cpp |  217 +++++++++++++++++++++++++++++++++++++---
 src/plugins/ros/pcl_thread.h   |   36 +++++++-
 3 files changed, 242 insertions(+), 15 deletions(-)


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

- *commit* 0516b09939734306a619d6cd879ec7b91416ff45 - - - - - - - - - -
Author:  Tobias Neumann <t.neumann at fh-aachen.de>
Date:    Tue Apr 12 14:42:56 2016 +0200
Subject: ros-pcl: add bidirectional exchange of pointclouds

 src/plugins/ros/pcl_thread.cpp |  210 +++++++++++++++++++++++++++++++++++++---
 src/plugins/ros/pcl_thread.h   |   34 ++++++-
 2 files changed, 231 insertions(+), 13 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/ros/pcl_thread.cpp b/src/plugins/ros/pcl_thread.cpp
index c8b752c..cd5588b 100644
--- a/src/plugins/ros/pcl_thread.cpp
+++ b/src/plugins/ros/pcl_thread.cpp
@@ -22,6 +22,7 @@
 #include "pcl_thread.h"
 
 #include <core/threading/mutex_locker.h>
+#include <ros/ros.h>
 #include <sensor_msgs/PointCloud2.h>
 
 using namespace fawkes;
@@ -51,6 +52,147 @@ RosPointCloudThread::init()
   //__pubman = new RosPointCloudPublisherManager(rosnode);
   __adapter = new PointCloudAdapter(pcl_manager, logger);
 
+  fawkes_pointcloud_search();
+
+  ros_pointcloud_search();
+  ros_pointcloud_last_searched.stamp();
+}
+
+
+void
+RosPointCloudThread::finalize()
+{
+  for (std::string item : ros_pointcloud_available) {
+    pcl_manager->remove_pointcloud(item.c_str());
+  }
+  for (std::pair<std::string, fawkes::pcl_utils::StorageAdapter *> item : ros_pointcloud_available_ref) {
+    delete item.second;
+  }
+  for (std::pair<std::string, ros::Subscriber> item : ros_pointcloud_subs) {
+    item.second.shutdown();
+  }
+  delete __adapter;
+}
+
+
+void
+RosPointCloudThread::loop()
+{
+  // search ever n sec for new clouds on ROS side
+  fawkes::Time now;
+  now.stamp();
+  if ( fawkes::time_diff_sec(*now.get_timeval(), *ros_pointcloud_last_searched.get_timeval()) >= 10.  ) {
+    ros_pointcloud_last_searched = now;
+    ros_pointcloud_search();
+    ros_pointcloud_check_for_listener_in_fawkes();
+    //TODO if fawkes_pointcloud_search() would be called here, a check for clouds from fawkes need to be implemented
+  }
+
+  // publish clouds fawkes->ros
+  fawkes_pointcloud_publish_to_ros();
+  // publish clouds ros->fawkes (this is done in callbacks)
+}
+
+void
+RosPointCloudThread::ros_pointcloud_search()
+{
+  std::list<std::string> ros_pointclouds_new;
+
+  // get all ROS topics
+  ros::master::V_TopicInfo ros_topics;
+  if ( ! ros::master::getTopics(ros_topics) ) {
+    logger->log_info(name(), "Coulnd't get available ROS topics");
+    return;
+  }
+
+  // iterate through all topics
+  for ( ros::master::TopicInfo info : ros_topics ) {
+    // only topics of type sensor_msgs/PointCloud2 are important
+    if ( 0 == info.datatype.compare("sensor_msgs/PointCloud2") ) {
+      // check if this is a topic comming from fawkes
+      bool topic_not_from_fawkes = true;
+      for ( std::pair<std::string, PublisherInfo> fawkes_cloud : fawkes_pubs) {
+        if ( 0 == info.name.compare( fawkes_cloud.second.pub.getTopic() ) ) {
+          topic_not_from_fawkes = false;
+        }
+      }
+      if (topic_not_from_fawkes) {
+        ros_pointclouds_new.push_back(info.name);
+      }
+    }
+  }
+
+  // check for removed clouds
+  std::list<std::string> items_to_remove;
+  for (std::string item_old : ros_pointcloud_available) {
+    bool exists = false;
+    for (std::string item_new : ros_pointclouds_new) {
+      if (0 == item_old.compare(item_new)) {
+        exists = true;
+        break;
+      }
+    }
+    if ( ! exists ) {
+      items_to_remove.push_back(item_old);
+    }
+  }
+  for (std::string item : items_to_remove) {
+    logger->log_info(name(), "Pointcloud %s is not available from ROS anymore", item.c_str());
+    ros_pointcloud_available.remove(item);
+  }
+
+  // check for new clouds
+  for (std::string ros_topic : ros_pointclouds_new) {
+    bool exists = false;
+    for (std::string in_list : ros_pointcloud_available) {
+      if (0 == ros_topic.compare(in_list)) {
+        exists = true;
+        break;
+      }
+    }
+    if ( ! exists ) {
+      logger->log_info(name(), "Pointcloud %s is now available from ROS", ros_topic.c_str());
+      ros_pointcloud_available.push_back(ros_topic);
+      ros_pointcloud_subs[ros_topic] = rosnode->subscribe<sensor_msgs::PointCloud2>(ros_topic, 1,
+          boost::bind(&RosPointCloudThread::ros_pointcloud_on_data_msg, this, _1, ros_topic)
+      );
+    }
+  }
+}
+
+void
+RosPointCloudThread::ros_pointcloud_check_for_listener_in_fawkes()
+{
+  for (std::pair<std::string, fawkes::pcl_utils::StorageAdapter *> item : ros_pointcloud_available_ref) {
+    unsigned int use_count = 0;
+    if (item.second->is_pointtype<pcl::PointXYZ>()) {
+      use_count = dynamic_cast<fawkes::pcl_utils::PointCloudStorageAdapter<pcl::PointXYZ> *>(item.second)->cloud.use_count();
+    } else if (item.second->is_pointtype<pcl::PointXYZRGB>()) {
+      use_count = dynamic_cast<fawkes::pcl_utils::PointCloudStorageAdapter<pcl::PointXYZRGB> *>(item.second)->cloud.use_count();
+    } else if (item.second->is_pointtype<pcl::PointXYZI>()) {
+      use_count = dynamic_cast<fawkes::pcl_utils::PointCloudStorageAdapter<pcl::PointXYZI> *>(item.second)->cloud.use_count();
+    } else {
+      logger->log_error(name(), "Can't detect cloud type");
+    }
+
+    logger->log_info(name(), "For: %s\t got %u", item.first.c_str(), use_count);
+    if ( use_count <= 2 ) { // my internal list, this ref and the pcl_manager have copys of this pointer, if more are used, otheres are listening too
+      std::map<std::string, ros::Subscriber>::iterator element = ros_pointcloud_subs.find(item.first);
+      if (element != ros_pointcloud_subs.end()) {
+        element->second.shutdown();
+        ros_pointcloud_subs.erase(item.first);
+      }
+    } else {
+      ros_pointcloud_subs[item.first] = rosnode->subscribe<sensor_msgs::PointCloud2>(item.first, 1,
+          boost::bind(&RosPointCloudThread::ros_pointcloud_on_data_msg, this, _1, item.first)
+      );
+    }
+  }
+}
+
+void
+RosPointCloudThread::fawkes_pointcloud_search()
+{
   std::vector<std::string> pcls = pcl_manager->get_pointcloud_list();
 
   std::vector<std::string>::iterator p;
@@ -85,23 +227,15 @@ RosPointCloudThread::init()
       pi.msg.fields[i].count    = fieldinfo[i].count;
     }
 
-    __pubs[*p] = pi;
+    fawkes_pubs[*p] = pi;
   }
 }
 
-
 void
-RosPointCloudThread::finalize()
-{
-  delete __adapter;
-}
-
-
-void
-RosPointCloudThread::loop()
+RosPointCloudThread::fawkes_pointcloud_publish_to_ros()
 {
   std::map<std::string, PublisherInfo>::iterator p;
-  for (p = __pubs.begin(); p != __pubs.end(); ++p) {
+  for (p = fawkes_pubs.begin(); p != fawkes_pubs.end(); ++p) {
     PublisherInfo &pi = p->second;
     if (pi.pub.getNumSubscribers() > 0) {
       unsigned int width, height;
@@ -122,7 +256,7 @@ RosPointCloudThread::loop()
 
         pi.msg.width             = width;
         pi.msg.height            = height;
-	pi.msg.header.frame_id   = frame_id;
+        pi.msg.header.frame_id   = frame_id;
         pi.msg.header.stamp.sec  = time.get_sec();
         pi.msg.header.stamp.nsec = time.get_nsec();
         pi.msg.point_step        = point_size;
@@ -137,3 +271,55 @@ RosPointCloudThread::loop()
     }
   }
 }
+
+void
+RosPointCloudThread::ros_pointcloud_on_data_msg(const sensor_msgs::PointCloud2ConstPtr &msg, const std::string topic_name)
+{
+  // if this is the first time, I need the meta infos, what point-type is send
+  if ( ! pcl_manager->exists_pointcloud( topic_name.c_str() ) ) {
+    bool r = false, i = false;
+    for (sensor_msgs::PointField field : msg->fields) {
+//      logger->log_info(name(), "%s: %s", topic_name.c_str(), field.name.c_str());
+      if ( 0 == field.name.compare("r") ) { r = true; }
+      if ( 0 == field.name.compare("i") ) { i = true; }
+    }
+    if ( !r && !i ) {
+      logger->log_info(name(), "%s: XYZ", topic_name.c_str());
+      add_pointcloud<pcl::PointXYZ>(msg, topic_name);
+    } else if ( r && !i ) {
+      logger->log_info(name(), "%s: XYZRGB", topic_name.c_str());
+      add_pointcloud<pcl::PointXYZRGB>(msg, topic_name);
+    } else if ( !r && i) {
+      logger->log_info(name(), "%s: XYRI", topic_name.c_str());
+      add_pointcloud<pcl::PointXYZI>(msg, topic_name);
+    } else {
+      logger->log_error(name(), "%s: can't detect point type, using XYZ", topic_name.c_str());
+      add_pointcloud<pcl::PointXYZ>(msg, topic_name);
+    }
+  }
+
+  // copy data
+  const pcl_utils::StorageAdapter* sa = pcl_manager->get_storage_adapter(topic_name.c_str());
+  if (sa->is_pointtype<pcl::PointXYZ>()) {
+    update_pointcloud<pcl::PointXYZ>(msg, topic_name);
+  } else if (sa->is_pointtype<pcl::PointXYZRGB>()) {
+    update_pointcloud<pcl::PointXYZRGB>(msg, topic_name);
+  } else if (sa->is_pointtype<pcl::PointXYZI>()) {
+    update_pointcloud<pcl::PointXYZI>(msg, topic_name);
+  } else {
+    logger->log_error(name(), "Can't detect cloud type");
+  }
+
+  ros_pointcloud_check_for_listener_in_fawkes();
+}
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/plugins/ros/pcl_thread.h b/src/plugins/ros/pcl_thread.h
index 71d16b9..d3e4f60 100644
--- a/src/plugins/ros/pcl_thread.h
+++ b/src/plugins/ros/pcl_thread.h
@@ -35,12 +35,16 @@
 #include <blackboard/interface_observer.h>
 #include <interfaces/TransformInterface.h>
 #include <core/threading/mutex.h>
+#include <utils/time/time.h>
 
 #include <list>
 #include <queue>
 
 #include <ros/node_handle.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 #include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
 
 class RosPointCloudThread
 : public fawkes::Thread,
@@ -63,6 +67,30 @@ class RosPointCloudThread
  protected: virtual void run() { Thread::run(); }
 
  private:
+  void ros_pointcloud_search();
+  void ros_pointcloud_check_for_listener_in_fawkes();
+  void fawkes_pointcloud_publish_to_ros();
+  void fawkes_pointcloud_search();
+  void ros_pointcloud_on_data_msg(const sensor_msgs::PointCloud2ConstPtr &msg, const std::string topic_name);
+
+  template<typename PointT>
+  void add_pointcloud(const sensor_msgs::PointCloud2ConstPtr &msg, const std::string topic_name)
+  {
+    fawkes::RefPtr<pcl::PointCloud<PointT> > pcl;
+    pcl = new pcl::PointCloud<PointT>();
+    pcl::fromROSMsg(*msg, **pcl);
+    pcl_manager->add_pointcloud(topic_name.c_str(), pcl);
+    ros_pointcloud_available_ref[topic_name] = new fawkes::pcl_utils::PointCloudStorageAdapter<PointT>(pcl);
+  }
+
+  template<typename PointT>
+  void update_pointcloud(const sensor_msgs::PointCloud2ConstPtr &msg, const std::string topic_name)
+  {
+    fawkes::RefPtr<pcl::PointCloud<PointT>> pcl;
+    pcl = dynamic_cast<fawkes::pcl_utils::PointCloudStorageAdapter<PointT> *>(ros_pointcloud_available_ref[topic_name])->cloud;
+    pcl::fromROSMsg(*msg, **pcl);
+  }
+
   PointCloudAdapter *__adapter;
 
   /// @cond INTERNALS
@@ -72,8 +100,12 @@ class RosPointCloudThread
     fawkes::Time             last_sent;
   } PublisherInfo;
   /// @endcond
-  std::map<std::string, PublisherInfo> __pubs;
+  std::map<std::string, PublisherInfo> fawkes_pubs;                            // the list and ref of topics from fawkes->ros
+  std::list<std::string> ros_pointcloud_available;                             // the list of topics from ros->fawkes
+  std::map<std::string, fawkes::pcl_utils::StorageAdapter *> ros_pointcloud_available_ref;// the list of refs of topics from ros->fawkes
+  std::map<std::string, ros::Subscriber> ros_pointcloud_subs;                  // the list of subscribers in ros, ros_pointcloud_available that are currently used in fawkes
 
+  fawkes::Time ros_pointcloud_last_searched;
 };
 
 #endif

- *commit* af30db550474cead809af16b02ec6093f4b0c4a8 - - - - - - - - - -
Author:  Tobias Neumann <t.neumann at fh-aachen.de>
Date:    Tue Apr 12 15:22:42 2016 +0200
Subject: ros-pcl: fixed segmentation fault (workaround?)

 src/plugins/ros/pcl_thread.cpp |    7 ++++---
 1 files changed, 4 insertions(+), 3 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/ros/pcl_thread.cpp b/src/plugins/ros/pcl_thread.cpp
index cd5588b..d69f377 100644
--- a/src/plugins/ros/pcl_thread.cpp
+++ b/src/plugins/ros/pcl_thread.cpp
@@ -175,7 +175,6 @@ RosPointCloudThread::ros_pointcloud_check_for_listener_in_fawkes()
       logger->log_error(name(), "Can't detect cloud type");
     }
 
-    logger->log_info(name(), "For: %s\t got %u", item.first.c_str(), use_count);
     if ( use_count <= 2 ) { // my internal list, this ref and the pcl_manager have copys of this pointer, if more are used, otheres are listening too
       std::map<std::string, ros::Subscriber>::iterator element = ros_pointcloud_subs.find(item.first);
       if (element != ros_pointcloud_subs.end()) {
@@ -237,7 +236,7 @@ RosPointCloudThread::fawkes_pointcloud_publish_to_ros()
   std::map<std::string, PublisherInfo>::iterator p;
   for (p = fawkes_pubs.begin(); p != fawkes_pubs.end(); ++p) {
     PublisherInfo &pi = p->second;
-    if (pi.pub.getNumSubscribers() > 0) {
+    if (pi.pub.getNumSubscribers() > 0 && pcl_manager->exists_pointcloud(p->first.c_str())) {
       unsigned int width, height;
       void *point_data;
       size_t point_size, num_points;
@@ -267,7 +266,9 @@ RosPointCloudThread::fawkes_pointcloud_publish_to_ros()
         // logger->log_debug(name(), "No update for %s, not sending", p->first.c_str());
       }
     } else {
-      __adapter->close(p->first);
+      if (pcl_manager->exists_pointcloud(p->first.c_str())) {
+        __adapter->close(p->first);
+      }
     }
   }
 }

- *commit* 5d86cb166eb4f0a3c81fd95cb4ca15bd8f430897 - - - - - - - - - -
Author:  Tobias Neumann <t.neumann at fh-aachen.de>
Date:    Mon May 9 17:11:57 2016 +0200
Subject: ros-pcl: add changed requested by Tim

 cfg/conf.d/ros.yaml            |    4 +++
 src/plugins/ros/pcl_thread.cpp |   54 ++++++++++++++++++++-------------------
 src/plugins/ros/pcl_thread.h   |   16 ++++++-----
 3 files changed, 41 insertions(+), 33 deletions(-)

_Diff for modified files_:
diff --git a/cfg/conf.d/ros.yaml b/cfg/conf.d/ros.yaml
index 05e9680..9827739 100644
--- a/cfg/conf.d/ros.yaml
+++ b/cfg/conf.d/ros.yaml
@@ -34,6 +34,10 @@ ros:
     # Frequency in Hz at which the clock should be published
     frequency: 100
 
+  pcl:
+    # The time interval which is used to search for new pointclouds in ROS.
+    # This is the maximum time until a pointcloud from ROS is visible and usable in FAWKES
+    ros-search-interval: 2.
 
   tf:
     # Use tf2 for communication with ROS.
diff --git a/src/plugins/ros/pcl_thread.cpp b/src/plugins/ros/pcl_thread.cpp
index d69f377..207cf61 100644
--- a/src/plugins/ros/pcl_thread.cpp
+++ b/src/plugins/ros/pcl_thread.cpp
@@ -52,23 +52,25 @@ RosPointCloudThread::init()
   //__pubman = new RosPointCloudPublisherManager(rosnode);
   __adapter = new PointCloudAdapter(pcl_manager, logger);
 
+  cfg_ros_research_ival_ = config->get_float("/ros/pcl/ros-search-interval");
+
   fawkes_pointcloud_search();
 
   ros_pointcloud_search();
-  ros_pointcloud_last_searched.stamp();
+  ros_pointcloud_last_searched_.stamp();
 }
 
 
 void
 RosPointCloudThread::finalize()
 {
-  for (std::string item : ros_pointcloud_available) {
+  for (const std::string &item : ros_pointcloud_available_) {
     pcl_manager->remove_pointcloud(item.c_str());
   }
-  for (std::pair<std::string, fawkes::pcl_utils::StorageAdapter *> item : ros_pointcloud_available_ref) {
+  for (const std::pair<std::string, fawkes::pcl_utils::StorageAdapter *> &item : ros_pointcloud_available_ref_) {
     delete item.second;
   }
-  for (std::pair<std::string, ros::Subscriber> item : ros_pointcloud_subs) {
+  for (std::pair<std::string, ros::Subscriber> item : ros_pointcloud_subs_) {
     item.second.shutdown();
   }
   delete __adapter;
@@ -81,8 +83,8 @@ RosPointCloudThread::loop()
   // search ever n sec for new clouds on ROS side
   fawkes::Time now;
   now.stamp();
-  if ( fawkes::time_diff_sec(*now.get_timeval(), *ros_pointcloud_last_searched.get_timeval()) >= 10.  ) {
-    ros_pointcloud_last_searched = now;
+  if ( fawkes::time_diff_sec(*now.get_timeval(), *ros_pointcloud_last_searched_.get_timeval()) >= cfg_ros_research_ival_  ) {
+    ros_pointcloud_last_searched_ = now;
     ros_pointcloud_search();
     ros_pointcloud_check_for_listener_in_fawkes();
     //TODO if fawkes_pointcloud_search() would be called here, a check for clouds from fawkes need to be implemented
@@ -106,12 +108,12 @@ RosPointCloudThread::ros_pointcloud_search()
   }
 
   // iterate through all topics
-  for ( ros::master::TopicInfo info : ros_topics ) {
+  for ( const ros::master::TopicInfo &info : ros_topics ) {
     // only topics of type sensor_msgs/PointCloud2 are important
     if ( 0 == info.datatype.compare("sensor_msgs/PointCloud2") ) {
       // check if this is a topic comming from fawkes
       bool topic_not_from_fawkes = true;
-      for ( std::pair<std::string, PublisherInfo> fawkes_cloud : fawkes_pubs) {
+      for ( const std::pair<std::string, PublisherInfo> &fawkes_cloud : fawkes_pubs_) {
         if ( 0 == info.name.compare( fawkes_cloud.second.pub.getTopic() ) ) {
           topic_not_from_fawkes = false;
         }
@@ -124,7 +126,7 @@ RosPointCloudThread::ros_pointcloud_search()
 
   // check for removed clouds
   std::list<std::string> items_to_remove;
-  for (std::string item_old : ros_pointcloud_available) {
+  for (const std::string &item_old : ros_pointcloud_available_) {
     bool exists = false;
     for (std::string item_new : ros_pointclouds_new) {
       if (0 == item_old.compare(item_new)) {
@@ -136,15 +138,15 @@ RosPointCloudThread::ros_pointcloud_search()
       items_to_remove.push_back(item_old);
     }
   }
-  for (std::string item : items_to_remove) {
+  for (const std::string &item : items_to_remove) {
     logger->log_info(name(), "Pointcloud %s is not available from ROS anymore", item.c_str());
-    ros_pointcloud_available.remove(item);
+    ros_pointcloud_available_.remove(item);
   }
 
   // check for new clouds
-  for (std::string ros_topic : ros_pointclouds_new) {
+  for (const std::string &ros_topic : ros_pointclouds_new) {
     bool exists = false;
-    for (std::string in_list : ros_pointcloud_available) {
+    for (const std::string &in_list : ros_pointcloud_available_) {
       if (0 == ros_topic.compare(in_list)) {
         exists = true;
         break;
@@ -152,8 +154,8 @@ RosPointCloudThread::ros_pointcloud_search()
     }
     if ( ! exists ) {
       logger->log_info(name(), "Pointcloud %s is now available from ROS", ros_topic.c_str());
-      ros_pointcloud_available.push_back(ros_topic);
-      ros_pointcloud_subs[ros_topic] = rosnode->subscribe<sensor_msgs::PointCloud2>(ros_topic, 1,
+      ros_pointcloud_available_.push_back(ros_topic);
+      ros_pointcloud_subs_[ros_topic] = rosnode->subscribe<sensor_msgs::PointCloud2>(ros_topic, 1,
           boost::bind(&RosPointCloudThread::ros_pointcloud_on_data_msg, this, _1, ros_topic)
       );
     }
@@ -163,7 +165,7 @@ RosPointCloudThread::ros_pointcloud_search()
 void
 RosPointCloudThread::ros_pointcloud_check_for_listener_in_fawkes()
 {
-  for (std::pair<std::string, fawkes::pcl_utils::StorageAdapter *> item : ros_pointcloud_available_ref) {
+  for (const std::pair<std::string, fawkes::pcl_utils::StorageAdapter *> &item : ros_pointcloud_available_ref_) {
     unsigned int use_count = 0;
     if (item.second->is_pointtype<pcl::PointXYZ>()) {
       use_count = dynamic_cast<fawkes::pcl_utils::PointCloudStorageAdapter<pcl::PointXYZ> *>(item.second)->cloud.use_count();
@@ -176,13 +178,13 @@ RosPointCloudThread::ros_pointcloud_check_for_listener_in_fawkes()
     }
 
     if ( use_count <= 2 ) { // my internal list, this ref and the pcl_manager have copys of this pointer, if more are used, otheres are listening too
-      std::map<std::string, ros::Subscriber>::iterator element = ros_pointcloud_subs.find(item.first);
-      if (element != ros_pointcloud_subs.end()) {
+      std::map<std::string, ros::Subscriber>::iterator element = ros_pointcloud_subs_.find(item.first);
+      if (element != ros_pointcloud_subs_.end()) {
         element->second.shutdown();
-        ros_pointcloud_subs.erase(item.first);
+        ros_pointcloud_subs_.erase(item.first);
       }
     } else {
-      ros_pointcloud_subs[item.first] = rosnode->subscribe<sensor_msgs::PointCloud2>(item.first, 1,
+      ros_pointcloud_subs_[item.first] = rosnode->subscribe<sensor_msgs::PointCloud2>(item.first, 1,
           boost::bind(&RosPointCloudThread::ros_pointcloud_on_data_msg, this, _1, item.first)
       );
     }
@@ -226,7 +228,7 @@ RosPointCloudThread::fawkes_pointcloud_search()
       pi.msg.fields[i].count    = fieldinfo[i].count;
     }
 
-    fawkes_pubs[*p] = pi;
+    fawkes_pubs_[*p] = pi;
   }
 }
 
@@ -234,7 +236,7 @@ void
 RosPointCloudThread::fawkes_pointcloud_publish_to_ros()
 {
   std::map<std::string, PublisherInfo>::iterator p;
-  for (p = fawkes_pubs.begin(); p != fawkes_pubs.end(); ++p) {
+  for (p = fawkes_pubs_.begin(); p != fawkes_pubs_.end(); ++p) {
     PublisherInfo &pi = p->second;
     if (pi.pub.getNumSubscribers() > 0 && pcl_manager->exists_pointcloud(p->first.c_str())) {
       unsigned int width, height;
@@ -279,19 +281,19 @@ RosPointCloudThread::ros_pointcloud_on_data_msg(const sensor_msgs::PointCloud2Co
   // if this is the first time, I need the meta infos, what point-type is send
   if ( ! pcl_manager->exists_pointcloud( topic_name.c_str() ) ) {
     bool r = false, i = false;
-    for (sensor_msgs::PointField field : msg->fields) {
+    for (const sensor_msgs::PointField &field : msg->fields) {
 //      logger->log_info(name(), "%s: %s", topic_name.c_str(), field.name.c_str());
       if ( 0 == field.name.compare("r") ) { r = true; }
       if ( 0 == field.name.compare("i") ) { i = true; }
     }
     if ( !r && !i ) {
-      logger->log_info(name(), "%s: XYZ", topic_name.c_str());
+      logger->log_info(name(), "Adding %s with type XYZ ROS -> FAWKES", topic_name.c_str());
       add_pointcloud<pcl::PointXYZ>(msg, topic_name);
     } else if ( r && !i ) {
-      logger->log_info(name(), "%s: XYZRGB", topic_name.c_str());
+      logger->log_info(name(), "Adding %s with type XYZRGB ROS -> FAWKES", topic_name.c_str());
       add_pointcloud<pcl::PointXYZRGB>(msg, topic_name);
     } else if ( !r && i) {
-      logger->log_info(name(), "%s: XYRI", topic_name.c_str());
+      logger->log_info(name(), "Adding %s with type XYRI ROS -> FAWKES", topic_name.c_str());
       add_pointcloud<pcl::PointXYZI>(msg, topic_name);
     } else {
       logger->log_error(name(), "%s: can't detect point type, using XYZ", topic_name.c_str());
diff --git a/src/plugins/ros/pcl_thread.h b/src/plugins/ros/pcl_thread.h
index d3e4f60..e95461b 100644
--- a/src/plugins/ros/pcl_thread.h
+++ b/src/plugins/ros/pcl_thread.h
@@ -80,14 +80,14 @@ class RosPointCloudThread
     pcl = new pcl::PointCloud<PointT>();
     pcl::fromROSMsg(*msg, **pcl);
     pcl_manager->add_pointcloud(topic_name.c_str(), pcl);
-    ros_pointcloud_available_ref[topic_name] = new fawkes::pcl_utils::PointCloudStorageAdapter<PointT>(pcl);
+    ros_pointcloud_available_ref_[topic_name] = new fawkes::pcl_utils::PointCloudStorageAdapter<PointT>(pcl);
   }
 
   template<typename PointT>
   void update_pointcloud(const sensor_msgs::PointCloud2ConstPtr &msg, const std::string topic_name)
   {
     fawkes::RefPtr<pcl::PointCloud<PointT>> pcl;
-    pcl = dynamic_cast<fawkes::pcl_utils::PointCloudStorageAdapter<PointT> *>(ros_pointcloud_available_ref[topic_name])->cloud;
+    pcl = dynamic_cast<fawkes::pcl_utils::PointCloudStorageAdapter<PointT> *>(ros_pointcloud_available_ref_[topic_name])->cloud;
     pcl::fromROSMsg(*msg, **pcl);
   }
 
@@ -100,12 +100,14 @@ class RosPointCloudThread
     fawkes::Time             last_sent;
   } PublisherInfo;
   /// @endcond
-  std::map<std::string, PublisherInfo> fawkes_pubs;                            // the list and ref of topics from fawkes->ros
-  std::list<std::string> ros_pointcloud_available;                             // the list of topics from ros->fawkes
-  std::map<std::string, fawkes::pcl_utils::StorageAdapter *> ros_pointcloud_available_ref;// the list of refs of topics from ros->fawkes
-  std::map<std::string, ros::Subscriber> ros_pointcloud_subs;                  // the list of subscribers in ros, ros_pointcloud_available that are currently used in fawkes
+  std::map<std::string, PublisherInfo> fawkes_pubs_;                           // the list and ref of topics from fawkes->ros
+  std::list<std::string> ros_pointcloud_available_;                            // the list of topics from ros->fawkes
+  std::map<std::string, fawkes::pcl_utils::StorageAdapter *> ros_pointcloud_available_ref_;// the list of refs of topics from ros->fawkes
+  std::map<std::string, ros::Subscriber> ros_pointcloud_subs_;                 // the list of subscribers in ros, ros_pointcloud_available that are currently used in fawkes
 
-  fawkes::Time ros_pointcloud_last_searched;
+  fawkes::Time ros_pointcloud_last_searched_;
+
+  float cfg_ros_research_ival_;
 };
 
 #endif




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


More information about the fawkes-commits mailing list