Added threadpool to laser plugin, fixed bug with plugin enabled param…

README.md

@@ -23,3 +23,12 @@
 * How to use: http://flatland-simulator.readthedocs.io
 * Doxygen: http://flatland-simulator-api.readthedocs.io
 * For a quick start use: https://github.com/avidbots/turtlebot_flatland
+
+### License ###
+All Flatland code is BSD 3-clause licensed (see LICENSE for details)
+
+Flatland uses a number of open source libraries that it includes in its source tree:
+- [ThreadPool](https://github.com/progschj/ThreadPool) Copyright (c) 2012 Jakob Progsch, Václav Zeman (zlib license)
+- [Tweeny](https://github.com/mobius3/tweeny) Copyright (c) 2016 Leonardo Guilherme de Freitas (MIT license)
+- [Box2d](https://github.com/erincatto/Box2D) Copyright (c) 2006-2017 Erin Catto [http://www.box2d.org](http://www.box2d.org) (zlib license)
+

flatland/package.xml

@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <package>
   <name>flatland</name>
-  <version>1.1.0</version>
+  <version>1.1.1</version>
   <description>
     This is the metapackage for flatland.
   </description>

flatland_msgs/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flatland_msgs</name>
-  <version>1.1.0</version>
+  <version>1.1.1</version>
   <description>The flatland_msgs package</description>
   <license>BSD</license>
   <url type="website">https://bitbucket.org/avidbots/flatland</url>

flatland_plugins/include/flatland_plugins/laser.h

@@ -51,8 +51,10 @@
 #include <ros/ros.h>
 #include <sensor_msgs/LaserScan.h>
 #include <tf/transform_broadcaster.h>
+#include <thirdparty/ThreadPool.h>
 #include <visualization_msgs/Marker.h>
 #include <Eigen/Dense>
+#include <thread>
 
 #ifndef FLATLAND_PLUGINS_LASER_H
 #define FLATLAND_PLUGINS_LASER_H
@@ -65,7 +67,7 @@ namespace flatland_plugins {
  * This class implements the model plugin class and provides laser data
  * for the given configurations
  */
-class Laser : public ModelPlugin, public b2RayCastCallback {
+class Laser : public ModelPlugin {
  public:
   std::string topic_;     ///< topic name to publish the laser scan
   Body *body_;            ///<  body the laser frame attaches to
@@ -79,6 +81,7 @@ class Laser : public ModelPlugin, public b2RayCastCallback {
   std::string frame_id_;  ///< laser frame id name
   bool broadcast_tf_;     ///< whether to broadcast laser origin w.r.t body
   uint16_t layers_bits_;  ///< for setting the layers where laser will function
+  ThreadPool pool_;       ///< ThreadPool for managing concurrent scan threads
 
   /*
    * for setting reflectance layers. if the laser hits those layers,
@@ -97,25 +100,19 @@ class Laser : public ModelPlugin, public b2RayCastCallback {
   Eigen::Vector3f v_zero_point_;          ///< point representing (0,0)
   Eigen::Vector3f v_world_laser_origin_;  ///< (0,0) in the laser frame
   sensor_msgs::LaserScan laser_scan_;     ///< for publishing laser scan
-  bool did_hit_;     ///< Box2D ray trace checking if ray hits anything
-  float fraction_;   ///< Box2D ray trace fraction
-  float intensity_;  ///< Intensity of raytrace collision
 
   ros::Publisher scan_publisher_;             ///< ros laser topic publisher
   tf::TransformBroadcaster tf_broadcaster_;   ///< broadcast laser frame
   geometry_msgs::TransformStamped laser_tf_;  ///< tf from body to laser frame
   UpdateTimer update_timer_;                  ///< for controlling update rate
 
   /**
-   * @brief Box2D raytrace call back method required for implementing the
-   * b2RayCastCallback abstract class
-   * @param[in] fixture Fixture the ray hits
-   * @param[in] point Point the ray hits the fixture
-   * @param[in] normal Vector indicating the normal at the point hit
-   * @param[in] fraction Fraction of ray length at hit point
+   * @brief Constructor to start the threadpool with N+1 threads
    */
-  float ReportFixture(b2Fixture *fixture, const b2Vec2 &point,
-                      const b2Vec2 &normal, float fraction) override;
+  Laser() : pool_(std::thread::hardware_concurrency() + 1) {
+    ROS_INFO_STREAM("Laser plugin loaded with "
+                    << (std::thread::hardware_concurrency() + 1) << " threads");
+  };
 
   /**
    * @brief Initialization for the plugin
@@ -140,6 +137,34 @@ class Laser : public ModelPlugin, public b2RayCastCallback {
    */
   void ParseParameters(const YAML::Node &config);
 };
+
+/**
+ * This class handles the b2RayCastCallback ReportFixture method
+ * allowing each thread to access its own callback object
+ */
+class LaserCallback : public b2RayCastCallback {
+ public:
+  bool did_hit_ = false;  ///< Box2D ray trace checking if ray hits anything
+  float fraction_ = 0;    ///< Box2D ray trace fraction
+  float intensity_ = 0;   ///< Intensity of raytrace collision
+  Laser *parent_;         ///< The parent Laser plugin
+
+  /**
+   * Default constructor to assign parent object
+   */
+  LaserCallback(Laser *parent) : parent_(parent){};
+
+  /**
+   * @brief Box2D raytrace call back method required for implementing the
+   * b2RayCastCallback abstract class
+   * @param[in] fixture Fixture the ray hits
+   * @param[in] point Point the ray hits the fixture
+   * @param[in] normal Vector indicating the normal at the point hit
+   * @param[in] fraction Fraction of ray length at hit point
+   */
+  float ReportFixture(b2Fixture *fixture, const b2Vec2 &point,
+                      const b2Vec2 &normal, float fraction) override;
+};
 };
 
 #endif

flatland_plugins/include/thirdparty/ThreadPool.h

@@ -0,0 +1,123 @@
+/*
+https://github.com/progschj/ThreadPool
+
+Copyright (c) 2012 Jakob Progsch, Václav Zeman
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any damages
+arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+   1. The origin of this software must not be misrepresented; you must not
+   claim that you wrote the original software. If you use this software
+   in a product, an acknowledgment in the product documentation would be
+   appreciated but is not required.
+
+   2. Altered source versions must be plainly marked as such, and must not be
+   misrepresented as being the original software.
+
+   3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+#ifndef THREAD_POOL_H
+#define THREAD_POOL_H
+
+#include <vector>
+#include <queue>
+#include <memory>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <future>
+#include <functional>
+#include <stdexcept>
+
+class ThreadPool {
+public:
+    ThreadPool(size_t);
+    template<class F, class... Args>
+    auto enqueue(F&& f, Args&&... args) 
+        -> std::future<typename std::result_of<F(Args...)>::type>;
+    ~ThreadPool();
+private:
+    // need to keep track of threads so we can join them
+    std::vector< std::thread > workers;
+    // the task queue
+    std::queue< std::function<void()> > tasks;
+
+    // synchronization
+    std::mutex queue_mutex;
+    std::condition_variable condition;
+    bool stop;
+};
+
+// the constructor just launches some amount of workers
+inline ThreadPool::ThreadPool(size_t threads)
+    :   stop(false)
+{
+    for(size_t i = 0;i<threads;++i)
+        workers.emplace_back(
+            [this]
+            {
+                for(;;)
+                {
+                    std::function<void()> task;
+
+                    {
+                        std::unique_lock<std::mutex> lock(this->queue_mutex);
+                        this->condition.wait(lock,
+                            [this]{ return this->stop || !this->tasks.empty(); });
+                        if(this->stop && this->tasks.empty())
+                            return;
+                        task = std::move(this->tasks.front());
+                        this->tasks.pop();
+                    }
+
+                    task();
+                }
+            }
+        );
+}
+
+// add new work item to the pool
+template<class F, class... Args>
+auto ThreadPool::enqueue(F&& f, Args&&... args) 
+    -> std::future<typename std::result_of<F(Args...)>::type>
+{
+    using return_type = typename std::result_of<F(Args...)>::type;
+
+    auto task = std::make_shared< std::packaged_task<return_type()> >(
+            std::bind(std::forward<F>(f), std::forward<Args>(args)...)
+        );
+
+    std::future<return_type> res = task->get_future();
+    {
+        std::unique_lock<std::mutex> lock(queue_mutex);
+
+        // don't allow enqueueing after stopping the pool
+        if(stop)
+            throw std::runtime_error("enqueue on stopped ThreadPool");
+
+        tasks.emplace([task](){ (*task)(); });
+    }
+    condition.notify_one();
+    return res;
+}
+
+// the destructor joins all threads
+inline ThreadPool::~ThreadPool()
+{
+    {
+        std::unique_lock<std::mutex> lock(queue_mutex);
+        stop = true;
+    }
+    condition.notify_all();
+    for(std::thread &worker: workers)
+        worker.join();
+}
+
+#endif

flatland_plugins/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flatland_plugins</name>
-  <version>1.1.0</version>
+  <version>1.1.1</version>
   <description>Default plugins for flatland</description>
   <license>BSD</license>
   <url type="website">https://bitbucket.org/avidbots/flatland</url>

flatland_plugins/src/laser.cpp

@@ -161,42 +161,54 @@ void Laser::ComputeLaserRanges() {
   v_world_laser_origin_ = m_world_to_laser_ * v_zero_point_;
 
   // Conver to Box2D data types
-  b2Vec2 laser_point;
   b2Vec2 laser_origin_point(v_world_laser_origin_(0), v_world_laser_origin_(1));
 
-  // loop through the laser points and call the Box2D world raycast
+  // Results vector
+  std::vector<std::future<std::pair<double, double>>> results(
+      laser_scan_.ranges.size());
+
+  // loop through the laser points and call the Box2D world raycast by
+  // enqueueing the callback
+  for (unsigned int i = 0; i < laser_scan_.ranges.size(); ++i) {
+    results[i] =
+        pool_.enqueue([i, this, laser_origin_point] {  // Lambda function
+          b2Vec2 laser_point;
+          laser_point.x = m_world_laser_points_(0, i);
+          laser_point.y = m_world_laser_points_(1, i);
+          LaserCallback cb(this);
+
+          GetModel()->GetPhysicsWorld()->RayCast(&cb, laser_origin_point,
+                                                 laser_point);
+
+          if (!cb.did_hit_) {
+            return std::make_pair<double, double>(NAN, 0);
+          } else {
+            return std::make_pair<double, double>(cb.fraction_ * this->range_,
+                                                  cb.intensity_);
+          }
+        });
+  }
+
+  // Unqueue all of the future'd results
   for (unsigned int i = 0; i < laser_scan_.ranges.size(); ++i) {
-    laser_point.x = m_world_laser_points_(0, i);
-    laser_point.y = m_world_laser_points_(1, i);
-
-    did_hit_ = false;
-    intensity_ = 0.0;
-
-    GetModel()->GetPhysicsWorld()->RayCast(this, laser_origin_point,
-                                           laser_point);
-
-    if (!did_hit_) {
-      laser_scan_.ranges[i] = NAN;
-      if (reflectance_layers_bits_) laser_scan_.intensities[i] = 0;
-    } else {
-      laser_scan_.ranges[i] = fraction_ * range_ + noise_gen_(rng_);
-      if (reflectance_layers_bits_) laser_scan_.intensities[i] = intensity_;
-    }
+    auto result = results[i].get();  // Pull the result from the future
+    laser_scan_.ranges[i] = result.first + this->noise_gen_(this->rng_);
+    if (reflectance_layers_bits_) laser_scan_.intensities[i] = result.second;
   }
 }
 
-float Laser::ReportFixture(b2Fixture *fixture, const b2Vec2 &point,
-                           const b2Vec2 &normal, float fraction) {
+float LaserCallback::ReportFixture(b2Fixture *fixture, const b2Vec2 &point,
+                                   const b2Vec2 &normal, float fraction) {
   uint16_t category_bits = fixture->GetFilterData().categoryBits;
   // only register hit in the specified layers
-  if (!(category_bits & layers_bits_)) {
+  if (!(category_bits & parent_->layers_bits_)) {
     return -1.0f;  // return -1 to ignore this hit
   }
 
   // Don't return on hitting sensors... they're not real
   if (fixture->IsSensor()) return -1.0f;
 
-  if (category_bits & reflectance_layers_bits_) {
+  if (category_bits & parent_->reflectance_layers_bits_) {
     intensity_ = 255.0;
   }
 

flatland_server/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flatland_server</name>
-  <version>1.1.0</version>
+  <version>1.1.1</version>
   <description>The flatland_server package</description>
   <license>BSD</license>
   <url type="website">https://bitbucket.org/avidbots/flatland</url>

flatland_server/src/plugin_manager.cpp

@@ -131,13 +131,15 @@ void PluginManager::LoadModelPlugin(Model *model, YamlReader &plugin_reader) {
                             << plugin_reader.Get<std::string>("enabled"));
   }
 
-  // remove the name and type of the YAML Node, the plugin does not need to know
+  // remove the name, type and enabled of the YAML Node, the plugin does not
+  // need to know
   // about these parameters, remove method is broken in yaml cpp 5.2, so we
   // create a new node and add everything
   YAML::Node yaml_node;
   for (const auto &k : plugin_reader.Node()) {
     if (k.first.as<std::string>() != "name" &&
-        k.first.as<std::string>() != "type") {
+        k.first.as<std::string>() != "type" &&
+        k.first.as<std::string>() != "enabled") {
       yaml_node[k.first] = k.second;
     }
   }

flatland_viz/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flatland_viz</name>
-  <version>1.1.0</version>
+  <version>1.1.1</version>
   <description>The flatland gui and visualization</description>
   <license>BSD</license>
   <url type="website">https://bitbucket.org/avidbots/flatland</url>