[Feature] Added intensities to laser scan plugin

flatland_plugins/include/flatland_plugins/laser.h

@@ -79,6 +79,9 @@ 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
+  uint16_t reflectance_layers_bits_;  ///< for setting reflectance layers. if
+                                      ///laser hits those layers, intensity will
+                                      ///be high
 
   std::default_random_engine rng_;              ///< random generator
   std::normal_distribution<double> noise_gen_;  ///< gaussian noise generator
@@ -91,8 +94,9 @@ 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
+  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

flatland_plugins/src/laser.cpp

@@ -102,7 +102,10 @@ void Laser::OnInitialize(const YAML::Node &config) {
   laser_scan_.range_min = 0;
   laser_scan_.range_max = range_;
   laser_scan_.ranges.resize(num_laser_points);
-  laser_scan_.intensities.resize(0);
+  if (reflectance_layers_bits_)
+    laser_scan_.intensities.resize(num_laser_points);
+  else
+    laser_scan_.intensities.resize(0);
   laser_scan_.header.seq = 0;
   laser_scan_.header.frame_id =
       tf::resolve("", GetModel()->NameSpaceTF(frame_id_));
@@ -167,28 +170,36 @@ void Laser::ComputeLaserRanges() {
     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_;
     }
   }
 }
 
 float Laser::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 (!(fixture->GetFilterData().categoryBits & layers_bits_)) {
+  if (!(category_bits & 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_) {
+    intensity_ = 255.0;
+  }
+
   did_hit_ = true;
   fraction_ = fraction;
 
@@ -234,6 +245,10 @@ void Laser::ParseParameters(const YAML::Node &config) {
                         boost::algorithm::join(invalid_layers, ",") + "}");
   }
 
+  std::vector<std::string> reflectance_layer = {"reflectance"};
+  reflectance_layers_bits_ =
+      GetModel()->GetCfr()->GetCategoryBits(reflectance_layer, &invalid_layers);
+
   // init the random number generators
   std::random_device rd;
   rng_ = std::default_random_engine(rd());
@@ -252,4 +267,4 @@ void Laser::ParseParameters(const YAML::Node &config) {
 }
 };
 
-PLUGINLIB_EXPORT_CLASS(flatland_plugins::Laser, flatland_server::ModelPlugin)
+PLUGINLIB_EXPORT_CLASS(flatland_plugins::Laser, flatland_server::ModelPlugin)

flatland_plugins/test/laser_test.cpp

@@ -208,6 +208,52 @@ TEST_F(LaserPluginTest, range_test) {
   EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p2->update_rate_;
   EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]);
 }
+/**
+ * Test the laser plugin for intensity configuration
+ */
+TEST_F(LaserPluginTest, intensity_test) {
+  world_yaml =
+      this_file_dir / fs::path("laser_tests/intensity_test/world.yaml");
+
+  Timekeeper timekeeper;
+  timekeeper.SetMaxStepSize(1.0);
+  w = World::MakeWorld(world_yaml.string());
+
+  ros::NodeHandle nh;
+  ros::Subscriber sub_1, sub_2, sub_3;
+  LaserPluginTest* obj = dynamic_cast<LaserPluginTest*>(this);
+  sub_1 = nh.subscribe("r/scan", 1, &LaserPluginTest::ScanFrontCb, obj);
+  sub_2 = nh.subscribe("scan_center", 1, &LaserPluginTest::ScanCenterCb, obj);
+  sub_3 = nh.subscribe("r/scan_back", 1, &LaserPluginTest::ScanBackCb, obj);
+
+  Laser* p1 = dynamic_cast<Laser*>(w->plugin_manager_.model_plugins_[0].get());
+  Laser* p2 = dynamic_cast<Laser*>(w->plugin_manager_.model_plugins_[1].get());
+  Laser* p3 = dynamic_cast<Laser*>(w->plugin_manager_.model_plugins_[2].get());
+
+  // let it spin for 10 times to make sure the message gets through
+  ros::WallRate rate(500);
+  for (int i = 0; i < 10; i++) {
+    w->Update(timekeeper);
+    ros::spinOnce();
+    rate.sleep();
+  }
+
+  // check scan returns
+  EXPECT_TRUE(ScanEq(scan_front, "r_laser_front", -M_PI / 2, M_PI / 2, M_PI / 2,
+                     0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3}, {0, 0, 0}));
+  EXPECT_TRUE(fltcmp(p1->update_rate_, std::numeric_limits<float>::infinity()))
+      << "Actual: " << p1->update_rate_;
+  EXPECT_EQ(p1->body_, w->models_[0]->bodies_[0]);
+  EXPECT_TRUE(ScanEq(scan_center, "r_center_laser", 0, 2 * M_PI, M_PI / 2, 0.0,
+                     0.0, 0.0, 5.0, {4.8, 4.7, 4.6, 4.9, 4.8},
+                     {0, 255, 0, 0, 0}));
+  EXPECT_TRUE(fltcmp(p2->update_rate_, 5000)) << "Actual: " << p2->update_rate_;
+  EXPECT_EQ(p2->body_, w->models_[0]->bodies_[0]);
+  EXPECT_TRUE(ScanEq(scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0,
+                     0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, {0, 0, 0, 0, 0}));
+  EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p2->update_rate_;
+  EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]);
+}
 
 /**
  * Checks the laser plugin will throw correct exception for invalid

flatland_plugins/test/laser_tests/intensity_test/map_1.yaml

@@ -0,0 +1,7 @@
+image: map_1.png
+resolution: 0.1
+origin: [0, 0, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

flatland_plugins/test/laser_tests/intensity_test/map_2.yaml

@@ -0,0 +1,7 @@
+image: map_2.png
+resolution: 0.1
+origin: [0, 0, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

flatland_plugins/test/laser_tests/intensity_test/map_reflectance.yaml

@@ -0,0 +1,7 @@
+image: map_reflectance.png
+resolution: 0.1
+origin: [0, 0, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

flatland_plugins/test/laser_tests/intensity_test/robot.model.yaml

@@ -0,0 +1,40 @@
+# Turtlebot
+
+bodies:  # List of named bodies
+  - name: base_link
+    pose: [0, 0, 0] 
+    type: dynamic
+    color: [1, 1, 0, 1]
+    footprints:
+      - type: circle
+        density: 1
+        center: [0, 0]
+        radius: 0.1
+
+plugins:
+  - type: Laser
+    name: laser_front
+    body: base_link
+    range: 5
+    angle: {min: -1.5707963267948966, max: 1.5707963267948966, increment: 1.5707963267948966}
+
+  - type: Laser
+    name: laser_center
+    topic: /scan_center
+    frame: center_laser
+    body: base_link
+    origin: [0, 0, 0]
+    range: 5
+    update_rate: 5000
+    angle: {min: 0, max: 6.283185307179586, increment: 1.5707963267948966}
+    layers: ["layer_1", "reflectance"]
+
+  - type: Laser
+    name: laser_back
+    topic: scan_back
+    body: base_link
+    origin: [-1, -1, 1.5707963267948966]
+    range: 4
+    update_rate: 1
+    angle: {min: 0, max: 6.283185307179586, increment: 1.5707963267948966}
+    layers: ["layer_2", "reflectance"]

flatland_plugins/test/laser_tests/intensity_test/world.yaml

@@ -0,0 +1,16 @@
+properties: {}
+layers: 
+  - name: "layer_1"
+    map: "map_1.yaml"
+    color: [0, 1, 0, 1]
+  - name: "layer_2"
+    map: "map_2.yaml"
+    color: [0, 1, 0, 1]
+  - name: "reflectance"
+    map: "map_reflectance.yaml"
+    color: [0, 1, 0, 1]
+models: 
+  - name: robot1
+    pose: [5, 5, 0]
+    model: robot.model.yaml
+    namespace: "r"