diff --git a/flatland_plugins/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h index 7f10cb9b..68c717b1 100644 --- a/flatland_plugins/include/flatland_plugins/laser.h +++ b/flatland_plugins/include/flatland_plugins/laser.h @@ -80,6 +80,12 @@ class Laser : public ModelPlugin, public b2RayCastCallback { bool broadcast_tf_; ///< whether to broadcast laser origin w.r.t body uint16_t layers_bits_; ///< for setting the layers where laser will function + /* + * for setting reflectance layers. if the laser hits those layers, + * intensity will be high (255) + */ + uint16_t reflectance_layers_bits_; + std::default_random_engine rng_; ///< random generator std::normal_distribution noise_gen_; ///< gaussian noise generator @@ -91,8 +97,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 @@ -135,4 +142,4 @@ class Laser : public ModelPlugin, public b2RayCastCallback { }; }; -#endif \ No newline at end of file +#endif diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index 3c45b724..c9dc508b 100644 --- a/flatland_plugins/src/laser.cpp +++ b/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 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) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(flatland_plugins::Laser, flatland_server::ModelPlugin) diff --git a/flatland_plugins/test/laser_test.cpp b/flatland_plugins/test/laser_test.cpp index 8b709a64..12aa3ae2 100644 --- a/flatland_plugins/test/laser_test.cpp +++ b/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(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(w->plugin_manager_.model_plugins_[0].get()); + Laser* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); + Laser* p3 = dynamic_cast(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::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 diff --git a/flatland_plugins/test/laser_tests/intensity_test/map_1.png b/flatland_plugins/test/laser_tests/intensity_test/map_1.png new file mode 100644 index 00000000..dc20f776 Binary files /dev/null and b/flatland_plugins/test/laser_tests/intensity_test/map_1.png differ diff --git a/flatland_plugins/test/laser_tests/intensity_test/map_1.yaml b/flatland_plugins/test/laser_tests/intensity_test/map_1.yaml new file mode 100644 index 00000000..e258ddc2 --- /dev/null +++ b/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 + diff --git a/flatland_plugins/test/laser_tests/intensity_test/map_2.png b/flatland_plugins/test/laser_tests/intensity_test/map_2.png new file mode 100644 index 00000000..3debc7c4 Binary files /dev/null and b/flatland_plugins/test/laser_tests/intensity_test/map_2.png differ diff --git a/flatland_plugins/test/laser_tests/intensity_test/map_2.yaml b/flatland_plugins/test/laser_tests/intensity_test/map_2.yaml new file mode 100644 index 00000000..0a3d64cb --- /dev/null +++ b/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 + diff --git a/flatland_plugins/test/laser_tests/intensity_test/map_reflectance.png b/flatland_plugins/test/laser_tests/intensity_test/map_reflectance.png new file mode 100644 index 00000000..18a8b870 Binary files /dev/null and b/flatland_plugins/test/laser_tests/intensity_test/map_reflectance.png differ diff --git a/flatland_plugins/test/laser_tests/intensity_test/map_reflectance.yaml b/flatland_plugins/test/laser_tests/intensity_test/map_reflectance.yaml new file mode 100644 index 00000000..e224db9a --- /dev/null +++ b/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 + diff --git a/flatland_plugins/test/laser_tests/intensity_test/robot.model.yaml b/flatland_plugins/test/laser_tests/intensity_test/robot.model.yaml new file mode 100644 index 00000000..baa32c38 --- /dev/null +++ b/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"] diff --git a/flatland_plugins/test/laser_tests/intensity_test/world.yaml b/flatland_plugins/test/laser_tests/intensity_test/world.yaml new file mode 100644 index 00000000..61a7e67a --- /dev/null +++ b/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"