From c97b1eafe68972d4f8bd98b503bb12a6e929a415 Mon Sep 17 00:00:00 2001 From: marbosjo Date: Tue, 10 Oct 2017 06:51:20 +0200 Subject: [PATCH 1/6] added intensity to laser plugin Now laser filters returns intensities if: a layer called "reflectance" is available. if "reflectance" layer is available, ray traces that collides with an element of this layer, intensity is then 255. Otherwise is 0. --- .../include/flatland_plugins/laser.h | 2 ++ flatland_plugins/src/laser.cpp | 23 +++++++++++++++---- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/flatland_plugins/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h index 7f10cb9b..a1e75ab0 100644 --- a/flatland_plugins/include/flatland_plugins/laser.h +++ b/flatland_plugins/include/flatland_plugins/laser.h @@ -79,6 +79,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 + 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 noise_gen_; ///< gaussian noise generator @@ -93,6 +94,7 @@ class Laser : public ModelPlugin, public b2RayCastCallback { 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 diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index 3c45b724..e33ae769 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,27 +170,35 @@ 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 (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) From c5b07ba1a4b9d22ce6dbcbb2f336934c85812eb3 Mon Sep 17 00:00:00 2001 From: marbosjo Date: Tue, 10 Oct 2017 06:53:46 +0200 Subject: [PATCH 2/6] added unit test to laser intensity method --- flatland_plugins/test/laser_test.cpp | 46 ++++++++++++++++++ .../test/laser_tests/intensity_test/map_1.png | Bin 0 -> 290 bytes .../laser_tests/intensity_test/map_1.yaml | 7 +++ .../test/laser_tests/intensity_test/map_2.png | Bin 0 -> 284 bytes .../laser_tests/intensity_test/map_2.yaml | 7 +++ .../intensity_test/map_reflectance.png | Bin 0 -> 311 bytes .../intensity_test/map_reflectance.yaml | 7 +++ .../intensity_test/robot.model.yaml | 40 +++++++++++++++ .../laser_tests/intensity_test/world.yaml | 16 ++++++ 9 files changed, 123 insertions(+) create mode 100644 flatland_plugins/test/laser_tests/intensity_test/map_1.png create mode 100644 flatland_plugins/test/laser_tests/intensity_test/map_1.yaml create mode 100644 flatland_plugins/test/laser_tests/intensity_test/map_2.png create mode 100644 flatland_plugins/test/laser_tests/intensity_test/map_2.yaml create mode 100644 flatland_plugins/test/laser_tests/intensity_test/map_reflectance.png create mode 100644 flatland_plugins/test/laser_tests/intensity_test/map_reflectance.yaml create mode 100644 flatland_plugins/test/laser_tests/intensity_test/robot.model.yaml create mode 100644 flatland_plugins/test/laser_tests/intensity_test/world.yaml 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 0000000000000000000000000000000000000000..dc20f776d139f02c798b16c5bf5e0afb66e3e73d GIT binary patch literal 290 zcmeAS@N?(olHy`uVBq!ia0vp^DImFdh=kX=|D_uY#$r9Iy66gHf+|;}h2Ir#G#FEq$h4Rdj3Rg-iCJ|`S(V&xXo zfshekcEko2ph(1q1Wqt{Pzc06*rW<3S-nAQR_wxZL@O1Ta JS?83{1OQXmT7v)p literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..3debc7c41fef346e1d6fef0754882e001fa0e2a9 GIT binary patch literal 284 zcmeAS@N?(olHy`uVBq!ia0vp^DImFdh=kX=|y;#=C31>|BU#>bRsq+ z925eRO{ySv6RS6v#(V*NppayVYeb22er|4RUI~M9QEFmIYKlU6 zW=V#EyQgnJcq5-UP|EIGBOx&FdH1$|NI=2{Ba@9l}vr5Lfqbq9j56- zZb~^Bq~Hx^XiiWCGek~I0yDT$PJ$V%Ks6u+6G$h8f8As5eZah5%tTQV P=u`$zS3j3^P6 Date: Tue, 10 Oct 2017 06:57:39 +0200 Subject: [PATCH 3/6] uncommented check of hitting another sensor it was a typo --- flatland_plugins/src/laser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index e33ae769..c9dc508b 100644 --- a/flatland_plugins/src/laser.cpp +++ b/flatland_plugins/src/laser.cpp @@ -194,7 +194,7 @@ float Laser::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, } // Don't return on hitting sensors... they're not real - //if (fixture->IsSensor()) return -1.0f; + if (fixture->IsSensor()) return -1.0f; if (category_bits & reflectance_layers_bits_) { intensity_ = 255.0; From d1fc68e20efff495f6a641dfc15f04e0671d2937 Mon Sep 17 00:00:00 2001 From: marbosjo Date: Tue, 10 Oct 2017 06:58:10 +0200 Subject: [PATCH 4/6] now passes clang-format-3.8 --- flatland_plugins/include/flatland_plugins/laser.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/flatland_plugins/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h index a1e75ab0..887b64df 100644 --- a/flatland_plugins/include/flatland_plugins/laser.h +++ b/flatland_plugins/include/flatland_plugins/laser.h @@ -79,7 +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 + 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 noise_gen_; ///< gaussian noise generator @@ -92,9 +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 - float intensity_; ///< Intensity of raytrace collision + 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 From a0d7694be9cdcedd788417a763fa92126d0a12fb Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 11 Oct 2017 09:33:50 -0400 Subject: [PATCH 5/6] Update laser.h --- flatland_plugins/include/flatland_plugins/laser.h | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/flatland_plugins/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h index 887b64df..ab91a73e 100644 --- a/flatland_plugins/include/flatland_plugins/laser.h +++ b/flatland_plugins/include/flatland_plugins/laser.h @@ -79,9 +79,12 @@ 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 + + /* + * 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 @@ -139,4 +142,4 @@ class Laser : public ModelPlugin, public b2RayCastCallback { }; }; -#endif \ No newline at end of file +#endif From a2c204992cae43ce9c9a50fda720d917e9fdccfb Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 11 Oct 2017 09:35:39 -0400 Subject: [PATCH 6/6] Update laser.h --- flatland_plugins/include/flatland_plugins/laser.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flatland_plugins/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h index ab91a73e..68c717b1 100644 --- a/flatland_plugins/include/flatland_plugins/laser.h +++ b/flatland_plugins/include/flatland_plugins/laser.h @@ -79,7 +79,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 - + /* * for setting reflectance layers. if the laser hits those layers, * intensity will be high (255)