[Fawkes Git] branch/tneumann/pcl-ros-bidirectional: ros-pcl: add changed requested by Tim

Tobias Neumann tobias.neumann at alumni.fh-aachen.de
Mon May 9 17:50:58 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, tneumann/pcl-ros-bidirectional has been updated
        to  05fb6e343fdefb3c073bc4275eba4e95e0c7d44f (commit)
      from  48a251843f99b59fb62612b55341e26b12a5faa9 (commit)

http://git.fawkesrobotics.org/fawkes.git/tneumann/pcl-ros-bidirectional

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 05fb6e343fdefb3c073bc4275eba4e95e0c7d44f
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: Mon May 9 17:11:57 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/05fb6e3
http://trac.fawkesrobotics.org/changeset/05fb6e3

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


- *Summary* -----------------------------------------------------------
 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(-)


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

- *commit* 05fb6e343fdefb3c073bc4275eba4e95e0c7d44f - - - - - - - - - -
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..40044e9 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-research-ival: 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..0095673 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-research-ival");
+
   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