Merge pull request #79 from avidbots/laser-scan-cherry-pick

Laser scan cherry pick

flatland_plugins/CMakeLists.txt

@@ -3,6 +3,7 @@ project(flatland_plugins)
 
 # Ensure we're using c++11
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+set(CMAKE_BUILD_TYPE RelWithDebInfo)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)

flatland_plugins/include/flatland_plugins/laser.h

@@ -73,14 +73,15 @@ class Laser : public ModelPlugin {
   std::string topic_;     ///< topic name to publish the laser scan
   Body *body_;            ///<  body the laser frame attaches to
   Pose origin_;           ///< laser frame w.r.t the body
-  double range_;          ///< laser max range
-  double noise_std_dev_;  ///< noise std deviation
-  double max_angle_;      /// < laser max angle
-  double min_angle_;      ///< laser min angle
-  double increment_;      ///< laser angle increment
-  double update_rate_;    ///< the rate laser scan will be published
+  float range_;           ///< laser max range
+  float noise_std_dev_;   ///< noise std deviation
+  float max_angle_;       /// < laser max angle
+  float min_angle_;       ///< laser min angle
+  float increment_;       ///< laser angle increment
+  float update_rate_;     ///< the rate laser scan will be published
   std::string frame_id_;  ///< laser frame id name
   bool broadcast_tf_;     ///< whether to broadcast laser origin w.r.t body
+  bool flipped_;          ///< whether the lidar is flipped
   uint16_t layers_bits_;  ///< for setting the layers where laser will function
   ThreadPool pool_;       ///< ThreadPool for managing concurrent scan threads
 
@@ -90,17 +91,19 @@ class Laser : public ModelPlugin {
    */
   uint16_t reflectance_layers_bits_;
 
-  std::default_random_engine rng_;              ///< random generator
-  std::normal_distribution<double> noise_gen_;  ///< gaussian noise generator
-
-  Eigen::Matrix3f m_body_to_laser_;       ///< tf from body to laser
-  Eigen::Matrix3f m_world_to_body_;       ///< tf  from world to body
-  Eigen::Matrix3f m_world_to_laser_;      ///< tf from world to laser
-  Eigen::MatrixXf m_laser_points_;        ///< laser points in the laser' frame
-  Eigen::MatrixXf m_world_laser_points_;  /// laser point in the world frame
-  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
+  std::default_random_engine rng_;             ///< random generator
+  std::normal_distribution<float> noise_gen_;  ///< gaussian noise generator
+
+  Eigen::Matrix3f m_body_to_laser_;        ///< tf from body to laser
+  Eigen::Matrix3f m_world_to_body_;        ///< tf  from world to body
+  Eigen::Matrix3f m_world_to_laser_;       ///< tf from world to laser
+  Eigen::MatrixXf m_laser_points_;         ///< laser points in the laser' frame
+  Eigen::MatrixXf m_world_laser_points_;   /// laser point in the world frame
+  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
+  std::vector<float> m_lastMaxFractions_;  ///< the robot move slowly when
+                                           /// comparing with to the scan rate
 
   ros::Publisher scan_publisher_;             ///< ros laser topic publisher
   tf::TransformBroadcaster tf_broadcaster_;   ///< broadcast laser frame

flatland_plugins/src/laser.cpp

@@ -59,7 +59,7 @@ using namespace flatland_server;
 
 namespace flatland_plugins {
 
-void Laser::OnInitialize(const YAML::Node &config) {
+void Laser::OnInitialize(const YAML::Node& config) {
   ParseParameters(config);
 
   update_timer_.SetRate(update_rate_);
@@ -78,6 +78,7 @@ void Laser::OnInitialize(const YAML::Node &config) {
   // initialize size for the matrix storing the laser points
   m_laser_points_ = Eigen::MatrixXf(3, num_laser_points);
   m_world_laser_points_ = Eigen::MatrixXf(3, num_laser_points);
+  m_lastMaxFractions_ = std::vector<float>(num_laser_points, 1.0);
   v_zero_point_ << 0, 0, 1;
 
   // pre-calculate the laser points w.r.t to the laser frame, since this never
@@ -127,15 +128,17 @@ void Laser::OnInitialize(const YAML::Node &config) {
   laser_tf_.transform.rotation.w = q.w();
 }
 
-void Laser::BeforePhysicsStep(const Timekeeper &timekeeper) {
+void Laser::BeforePhysicsStep(const Timekeeper& timekeeper) {
   // keep the update rate
   if (!update_timer_.CheckUpdate(timekeeper)) {
     return;
   }
 
   // only compute and publish when the number of subscribers is not zero
   if (scan_publisher_.getNumSubscribers() > 0) {
+    //START_PROFILE(timekeeper, "compute laser range");
     ComputeLaserRanges();
+    //END_PROFILE(timekeeper, "compute laser range");
     laser_scan_.header.stamp = timekeeper.GetSimTime();
     scan_publisher_.publish(laser_scan_);
   }
@@ -150,7 +153,7 @@ void Laser::ComputeLaserRanges() {
   // get the transformation matrix from the world to the body, and get the
   // world to laser frame transformation matrix by multiplying the world to body
   // and body to laser
-  const b2Transform &t = body_->GetPhysicsBody()->GetTransform();
+  const b2Transform& t = body_->GetPhysicsBody()->GetTransform();
   m_world_to_body_ << t.q.c, -t.q.s, t.p.x, t.q.s, t.q.c, t.p.y, 0, 0, 1;
   m_world_to_laser_ = m_world_to_body_ * m_body_to_laser_;
 
@@ -163,42 +166,116 @@ void Laser::ComputeLaserRanges() {
   // Conver to Box2D data types
   b2Vec2 laser_origin_point(v_world_laser_origin_(0), v_world_laser_origin_(1));
 
-  // 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_);
-          }
-        });
+  // Results vector : clustered to avoid too many locks
+  constexpr size_t clusterSize = 10;
+  // The last cluster may have a different size
+  size_t lastClusterSize = laser_scan_.ranges.size() % clusterSize;
+  size_t nbCluster =
+      laser_scan_.ranges.size() / clusterSize + (lastClusterSize == 0 ? 0 : 1);
+  if (lastClusterSize == 0) {
+    lastClusterSize = clusterSize;
+  }
+  std::vector<std::future<std::vector<std::pair<float, float>>>> results(
+      nbCluster);
+
+  // At scan n    : we hit an obstacle at distance range_ * coef
+  // At scan n+1  : we try a first scan with length = range_ * coef *
+  // lastScanExpansion
+  // lastScanExpansion makes it possible to take into account of the motion
+  // and the rotation of the laser
+  const float lastScanExpansion =
+      1.0f + std::max<float>(4.0f / (range_ * update_rate_), 0.05f);
+
+  // loop through the laser points and call the Box2D world raycast
+  // by enqueueing the callback
+  for (unsigned int i = 0; i < nbCluster; ++i) {
+    size_t currentClusterSize =
+        (i == nbCluster - 1 ? lastClusterSize : clusterSize);
+    results[i] = pool_.enqueue([i, currentClusterSize, clusterSize, this,
+                                laser_origin_point, lastScanExpansion] {
+      std::vector<std::pair<float, float>> out(currentClusterSize);
+      size_t currentIndice = i * clusterSize;
+      // Iterating over the currentClusterSize indices, starting at i *
+      // clusterSize
+      for (size_t j = 0; j < currentClusterSize; ++j, ++currentIndice) {
+        b2Vec2 laser_point;
+        laser_point.x = m_world_laser_points_(0, currentIndice);
+        laser_point.y = m_world_laser_points_(1, currentIndice);
+        LaserCallback cb(this);
+
+        // 1 - Take advantage of the last scan : we may hit the same obstacle
+        // (if any),
+        // with the first part of the ray being [0 fraction] * range_
+        float fraction = std::min<float>(
+            lastScanExpansion * m_lastMaxFractions_[currentIndice], 1.0f);
+        GetModel()->GetPhysicsWorld()->RayCast(&cb, laser_origin_point,
+                                               laser_point, fraction);
+
+        // 2 - If no hit detected, we relaunch the scan
+        // with the second part of the ray being [fraction 1] * range_
+        if (!cb.did_hit_ && fraction < 1.0f) {
+          b2Vec2 new_origin_point =
+              fraction * laser_point + (1 - fraction) * laser_origin_point;
+          GetModel()->GetPhysicsWorld()->RayCast(&cb, new_origin_point,
+                                                 laser_point, 1.0f);
+          if (cb.did_hit_)
+            cb.fraction_ = fraction + cb.fraction_ - cb.fraction_ * fraction;
+        }
+
+        // 3 - Let's check the result
+        if (cb.did_hit_) {
+          m_lastMaxFractions_[currentIndice] = cb.fraction_;
+          out[j] = std::make_pair<float, float>(
+              cb.fraction_ * this->range_ + this->noise_gen_(this->rng_),
+              static_cast<float>(cb.intensity_));
+        } else {
+          m_lastMaxFractions_[currentIndice] = cb.fraction_;
+          out[j] = std::make_pair<float, float>(NAN, 0);
+        }
+      }
+      return out;
+    });
   }
 
   // Unqueue all of the future'd results
-  for (unsigned int i = 0; i < laser_scan_.ranges.size(); ++i) {
-    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;
+  const auto reflectance = reflectance_layers_bits_;
+  if (flipped_) {
+    auto i = laser_scan_.intensities.rbegin();
+    auto r = laser_scan_.ranges.rbegin();
+    for (auto clusterIte = results.begin(); clusterIte != results.end();
+         ++clusterIte) {
+      auto resultCluster = clusterIte->get();
+      for (auto ite = resultCluster.begin(); ite != resultCluster.end();
+           ++ite, ++i, ++r) {
+        // Loop unswitching should occur
+        if (reflectance) {
+          *i = ite->second;
+          *r = ite->first;
+        } else
+          *r = ite->first;
+      }
+    }
+  } else {
+    auto i = laser_scan_.intensities.begin();
+    auto r = laser_scan_.ranges.begin();
+    for (auto clusterIte = results.begin(); clusterIte != results.end();
+         ++clusterIte) {
+      auto resultCluster = clusterIte->get();
+      for (auto ite = resultCluster.begin(); ite != resultCluster.end();
+           ++ite, ++i, ++r) {
+        // Loop unswitching should occur
+        if (reflectance) {
+          *i = ite->second;
+          *r = ite->first;
+        } else
+          *r = ite->first;
+      }
+    }
   }
 }
 
-float LaserCallback::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 & parent_->layers_bits_)) {
@@ -218,7 +295,7 @@ float LaserCallback::ReportFixture(b2Fixture *fixture, const b2Vec2 &point,
   return fraction;
 }
 
-void Laser::ParseParameters(const YAML::Node &config) {
+void Laser::ParseParameters(const YAML::Node& config) {
   YamlReader reader(config);
   std::string body_name = reader.Get<std::string>("body");
   topic_ = reader.Get<std::string>("topic", "scan");
@@ -230,6 +307,8 @@ void Laser::ParseParameters(const YAML::Node &config) {
   range_ = reader.Get<double>("range");
   noise_std_dev_ = reader.Get<double>("noise_std_dev", 0);
 
+  flipped_ = reader.Get<bool>("flipped", false);
+
   std::vector<std::string> layers =
       reader.GetList<std::string>("layers", {"all"}, -1, -1);
 
@@ -264,7 +343,7 @@ void Laser::ParseParameters(const YAML::Node &config) {
   // init the random number generators
   std::random_device rd;
   rng_ = std::default_random_engine(rd());
-  noise_gen_ = std::normal_distribution<double>(0.0, noise_std_dev_);
+  noise_gen_ = std::normal_distribution<float>(0.0, noise_std_dev_);
 
   ROS_DEBUG_NAMED("LaserPlugin",
                   "Laser %s params: topic(%s) body(%s, %p) origin(%f,%f,%f) "
@@ -277,6 +356,6 @@ void Laser::ParseParameters(const YAML::Node &config) {
                   min_angle_, max_angle_, increment_, layers_bits_,
                   boost::algorithm::join(layers, ",").c_str());
 }
-};
+};  // namespace flatland_plugins
 
 PLUGINLIB_EXPORT_CLASS(flatland_plugins::Laser, flatland_server::ModelPlugin)

flatland_plugins/src/tricycle_drive.cpp

@@ -105,8 +105,8 @@ void TricycleDrive::OnInitialize(const YAML::Node& config) {
   angular_dynamics_.Configure(r.SubnodeOpt("angular_dynamics", YamlReader::MAP).Node());
 
   // Accept old configuration location for angular dynamics constraints if present
-  if (angular_dynamics_.velocity_limit_ != 0.0) angular_dynamics_.velocity_limit_ = r.Get<double>("max_angular_velocity", 0.0);
-  if (angular_dynamics_.acceleration_limit_ != 0.0) {
+  if (angular_dynamics_.velocity_limit_ == 0.0) angular_dynamics_.velocity_limit_ = r.Get<double>("max_angular_velocity", 0.0);
+  if (angular_dynamics_.acceleration_limit_ == 0.0) {
     angular_dynamics_.acceleration_limit_ = r.Get<double>("max_steer_acceleration", 0.0);
     angular_dynamics_.deceleration_limit_ = angular_dynamics_.acceleration_limit_ ;
   }

flatland_server/CMakeLists.txt

@@ -4,6 +4,8 @@ project(flatland_server)
 # Ensure we're using c++11
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 
+set(CMAKE_BUILD_TYPE RelWithDebInfo)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -176,6 +178,10 @@ install(FILES flatland_plugins.xml
     DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
+install(PROGRAMS scripts/map_to_lines.py
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 #############
 ## Testing ##
 #############

flatland_server/thirdparty/Box2D/Dynamics/b2World.cpp

@@ -1016,13 +1016,13 @@ struct b2WorldRayCastWrapper
 	b2RayCastCallback* callback;
 };
 
-void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const
+void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2, float maxFraction/*=1.0*/) const
 {
 	b2WorldRayCastWrapper wrapper;
 	wrapper.broadPhase = &m_contactManager.m_broadPhase;
 	wrapper.callback = callback;
 	b2RayCastInput input;
-	input.maxFraction = 1.0f;
+	input.maxFraction = maxFraction;
 	input.p1 = point1;
 	input.p2 = point2;
 	m_contactManager.m_broadPhase.RayCast(&wrapper, input);

flatland_server/thirdparty/Box2D/Dynamics/b2World.h

@@ -119,7 +119,7 @@ class b2World
 	/// @param callback a user implemented callback class.
 	/// @param point1 the ray starting point
 	/// @param point2 the ray ending point
-	void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const;
+	void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2, float maxFraction=1.0f) const;
 
 	/// Get the world body list. With the returned body, use b2Body::GetNext to get
 	/// the next body in the world list. A nullptr body indicates the end of the list.

scripts/clang_tidy_ignore.yaml

@@ -3,3 +3,4 @@
 - ".*/flatland/flatland_plugins/.*warning: 'log_deprecated' is deprecated \\[clang-diagnostic-deprecated-declarations\\].*"
 - ".*QtCore/qobject.h:235:16.*warning: Potential memory leak.*\\[clang-analyzer-cplusplus.NewDeleteLeaks\\].*"
 - ".*/flatland/flatland_viz/.*warning: 'log_deprecated' is deprecated \\[clang-diagnostic-deprecated-declarations\\].*"
+- ".*/eigen3/Eigen/.*warning.*"