From 02d5c13441d0396734876d4778644b4fe3a9d657 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 18 Jan 2023 17:05:51 -0500 Subject: [PATCH 01/15] improved wording on scan direction laser plugin option. Added documentation and a unit test. --- docs/included_plugins/laser.rst | 5 +++ .../include/flatland_plugins/laser.h | 2 +- flatland_plugins/src/laser.cpp | 4 +-- flatland_plugins/test/laser_test.cpp | 35 +++++++++++++++++++ .../range_test/robot.model.cw.yaml | 20 +++++++++++ .../test/laser_tests/range_test/world.cw.yaml | 13 +++++++ 6 files changed, 76 insertions(+), 3 deletions(-) create mode 100644 flatland_plugins/test/laser_tests/range_test/robot.model.cw.yaml create mode 100644 flatland_plugins/test/laser_tests/range_test/world.cw.yaml diff --git a/docs/included_plugins/laser.rst b/docs/included_plugins/laser.rst index d94f6e72..644f1f9a 100644 --- a/docs/included_plugins/laser.rst +++ b/docs/included_plugins/laser.rst @@ -55,6 +55,11 @@ messages. # lasers only detects objects in the specified layers layers: ["all"] + # optional, set the scan direction of the lidar sweep + # "clockwise" or "counter-clockwise", defaults to "counter-closewise" + scan_direction: "counter-clockwise" + + # another example - type: Laser name: laser_front diff --git a/flatland_plugins/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h index 6e8aff04..6088fdcb 100644 --- a/flatland_plugins/include/flatland_plugins/laser.h +++ b/flatland_plugins/include/flatland_plugins/laser.h @@ -81,7 +81,7 @@ class Laser : public ModelPlugin { 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 + bool scan_clockwise_; ///< whether the lidar is scan_direction uint16_t layers_bits_; ///< for setting the layers where laser will function ThreadPool pool_; ///< ThreadPool for managing concurrent scan threads diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index b60d1f75..749f17ee 100644 --- a/flatland_plugins/src/laser.cpp +++ b/flatland_plugins/src/laser.cpp @@ -239,7 +239,7 @@ void Laser::ComputeLaserRanges() { // Unqueue all of the future'd results const auto reflectance = reflectance_layers_bits_; - if (flipped_) { + if (scan_clockwise_) { auto i = laser_scan_.intensities.rbegin(); auto r = laser_scan_.ranges.rbegin(); for (auto clusterIte = results.begin(); clusterIte != results.end(); @@ -307,7 +307,7 @@ void Laser::ParseParameters(const YAML::Node& config) { range_ = reader.Get("range"); noise_std_dev_ = reader.Get("noise_std_dev", 0); - flipped_ = reader.Get("flipped", false); + scan_clockwise_ = reader.Get("scan_direction", "counter-clockwise").compare("clockwise") ? false : true; std::vector layers = reader.GetList("layers", {"all"}, -1, -1); diff --git a/flatland_plugins/test/laser_test.cpp b/flatland_plugins/test/laser_test.cpp index ba687266..e3163e0e 100644 --- a/flatland_plugins/test/laser_test.cpp +++ b/flatland_plugins/test/laser_test.cpp @@ -208,6 +208,41 @@ 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's ability to flip the scan direction to clockwise works as expected + */ +TEST_F(LaserPluginTest, scan_direction_test) { + world_yaml = this_file_dir / fs::path("laser_tests/range_test/world.cw.yaml"); + + Timekeeper timekeeper; + timekeeper.SetMaxStepSize(1.0); + w = World::MakeWorld(world_yaml.string()); + + ros::NodeHandle nh; + ros::Subscriber sub_1; + LaserPluginTest* obj = dynamic_cast(this); + sub_1 = nh.subscribe("r/scan", 1, &LaserPluginTest::ScanFrontCb, obj); + + Laser* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + + // let it spin for 10 times to make sure the message gets through + ros::WallRate rate(500); + for (unsigned 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.3, 4.4, 4.5}, {})); + EXPECT_TRUE(fltcmp(p1->update_rate_, std::numeric_limits::infinity())) + << "Actual: " << p1->update_rate_; + EXPECT_EQ(p1->body_, w->models_[0]->bodies_[0]); +} + /** * Test the laser plugin for intensity configuration */ diff --git a/flatland_plugins/test/laser_tests/range_test/robot.model.cw.yaml b/flatland_plugins/test/laser_tests/range_test/robot.model.cw.yaml new file mode 100644 index 00000000..d24b22e0 --- /dev/null +++ b/flatland_plugins/test/laser_tests/range_test/robot.model.cw.yaml @@ -0,0 +1,20 @@ +# 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 + scan_direction: clockwise + angle: {min: -1.5707963267948966, max: 1.5707963267948966, increment: 1.5707963267948966} diff --git a/flatland_plugins/test/laser_tests/range_test/world.cw.yaml b/flatland_plugins/test/laser_tests/range_test/world.cw.yaml new file mode 100644 index 00000000..44738ffa --- /dev/null +++ b/flatland_plugins/test/laser_tests/range_test/world.cw.yaml @@ -0,0 +1,13 @@ +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] +models: + - name: robot1 + pose: [5, 5, 0] + model: robot.model.cw.yaml + namespace: "r" From 2267d301d64167475a34d2e545c61b9d680f4135 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 18 Jan 2023 17:11:10 -0500 Subject: [PATCH 02/15] added ros industrial ci test runner github action --- .github/workflows/industrial_ci_action.yml | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 .github/workflows/industrial_ci_action.yml diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml new file mode 100644 index 00000000..16f27fe8 --- /dev/null +++ b/.github/workflows/industrial_ci_action.yml @@ -0,0 +1,16 @@ +name: CI + +on: [push, pull_request] + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: kinetic, UBUNTU: 16.04} + - {ROS_DISTRO: noetic, UBUNTU: 20.04} + runs-on: ubuntu-${{matrix.env.UBUNTU}} + steps: + - uses: actions/checkout@v3 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} \ No newline at end of file From b72df50f50d6fe1b10f5141f59e5e1890c144687 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 18 Jan 2023 18:10:11 -0500 Subject: [PATCH 03/15] only kinetic ci for now --- .github/workflows/industrial_ci_action.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 16f27fe8..7061dade 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -8,9 +8,9 @@ jobs: matrix: env: - {ROS_DISTRO: kinetic, UBUNTU: 16.04} - - {ROS_DISTRO: noetic, UBUNTU: 20.04} +# - {ROS_DISTRO: noetic, UBUNTU: 20.04} runs-on: ubuntu-${{matrix.env.UBUNTU}} steps: - uses: actions/checkout@v3 - uses: 'ros-industrial/industrial_ci@master' - env: ${{matrix.env}} \ No newline at end of file + env: ${{matrix.env}} From 6e8471032962f8da8342d159172b74a156046d87 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 19 Jan 2023 09:08:19 -0500 Subject: [PATCH 04/15] only kinetic ci for now --- .github/workflows/industrial_ci_action.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 7061dade..dce02fb1 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -8,7 +8,6 @@ jobs: matrix: env: - {ROS_DISTRO: kinetic, UBUNTU: 16.04} -# - {ROS_DISTRO: noetic, UBUNTU: 20.04} runs-on: ubuntu-${{matrix.env.UBUNTU}} steps: - uses: actions/checkout@v3 From b5dc1409cfe5575329bd1f0fee41191be1aeab60 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 19 Jan 2023 10:28:34 -0500 Subject: [PATCH 05/15] Update industrial_ci_action.yml --- .github/workflows/industrial_ci_action.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index dce02fb1..aa78cdba 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -7,7 +7,7 @@ jobs: strategy: matrix: env: - - {ROS_DISTRO: kinetic, UBUNTU: 16.04} + - {ROS_DISTRO: noetic, UBUNTU: 20.04} runs-on: ubuntu-${{matrix.env.UBUNTU}} steps: - uses: actions/checkout@v3 From b1e0408b7602d2d0fbf80370a2a70df1ebd18a39 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 19 Jan 2023 12:15:09 -0500 Subject: [PATCH 06/15] Update industrial_ci_action.yml --- .github/workflows/industrial_ci_action.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index aa78cdba..bcd4ed1a 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -9,6 +9,7 @@ jobs: env: - {ROS_DISTRO: noetic, UBUNTU: 20.04} runs-on: ubuntu-${{matrix.env.UBUNTU}} + container: ros:${{matrix.env.ROS_DISTRO}} steps: - uses: actions/checkout@v3 - uses: 'ros-industrial/industrial_ci@master' From 83d16169e1bab5729d39fba09acca42606886a81 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 19 Jan 2023 12:21:13 -0500 Subject: [PATCH 07/15] Update industrial_ci_action.yml --- .github/workflows/industrial_ci_action.yml | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index bcd4ed1a..ad06a32a 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -7,10 +7,9 @@ jobs: strategy: matrix: env: - - {ROS_DISTRO: noetic, UBUNTU: 20.04} - runs-on: ubuntu-${{matrix.env.UBUNTU}} - container: ros:${{matrix.env.ROS_DISTRO}} + - {ROS_DISTRO: noetic, ROS_REPO: main} + runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v1 - uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} From 50b1f40d2bca1a6df6aa20b836582028cf222eaf Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 19 Jan 2023 15:07:49 -0500 Subject: [PATCH 08/15] added extra time for services to start --- flatland_server/test/service_manager_test.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/flatland_server/test/service_manager_test.cpp b/flatland_server/test/service_manager_test.cpp index 923756ca..8d9a9529 100644 --- a/flatland_server/test/service_manager_test.cpp +++ b/flatland_server/test/service_manager_test.cpp @@ -118,7 +118,7 @@ TEST_F(ServiceManagerTest, spawn_valid_model) { // Threading is required since client.call blocks executing until return StartSimulationThread(); - ros::service::waitForService("spawn_model", 1000); + ros::service::waitForService("spawn_model", 5000); ASSERT_TRUE(client.call(srv)); ASSERT_TRUE(srv.response.success); @@ -157,7 +157,7 @@ TEST_F(ServiceManagerTest, spawn_invalid_model) { StartSimulationThread(); - ros::service::waitForService("spawn_model", 1000); + ros::service::waitForService("spawn_model", 5000); ASSERT_TRUE(client.call(srv)); ASSERT_FALSE(srv.response.success); @@ -189,7 +189,7 @@ TEST_F(ServiceManagerTest, move_model) { StartSimulationThread(); - ros::service::waitForService("move_model", 1000); + ros::service::waitForService("move_model", 5000); ASSERT_TRUE(client.call(srv)); ASSERT_TRUE(srv.response.success); @@ -219,7 +219,7 @@ TEST_F(ServiceManagerTest, move_nonexistent_model) { StartSimulationThread(); - ros::service::waitForService("move_model", 1000); + ros::service::waitForService("move_model", 5000); ASSERT_TRUE(client.call(srv)); ASSERT_FALSE(srv.response.success); @@ -243,7 +243,7 @@ TEST_F(ServiceManagerTest, delete_model) { StartSimulationThread(); - ros::service::waitForService("delete_model", 1000); + ros::service::waitForService("delete_model", 5000); ASSERT_TRUE(client.call(srv)); ASSERT_TRUE(srv.response.success); @@ -270,7 +270,7 @@ TEST_F(ServiceManagerTest, delete_nonexistent_model) { StartSimulationThread(); - ros::service::waitForService("delete_model", 1000); + ros::service::waitForService("delete_model", 5000); ASSERT_TRUE(client.call(srv)); ASSERT_FALSE(srv.response.success); From 8874c0a63f9407e9a3c735f5896d1b6776d0da9d Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 19 Jan 2023 16:46:06 -0500 Subject: [PATCH 09/15] Update laser.rst --- docs/included_plugins/laser.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/included_plugins/laser.rst b/docs/included_plugins/laser.rst index 644f1f9a..92cf7978 100644 --- a/docs/included_plugins/laser.rst +++ b/docs/included_plugins/laser.rst @@ -56,7 +56,7 @@ messages. layers: ["all"] # optional, set the scan direction of the lidar sweep - # "clockwise" or "counter-clockwise", defaults to "counter-closewise" + # "clockwise" or "counter-clockwise", defaults to "counter-clockwise" scan_direction: "counter-clockwise" @@ -69,4 +69,4 @@ messages. layers: ["layer_1", "layer_2", "layer_3"] update_rate: 100 noise_std_dev: 0.01 - \ No newline at end of file + From c4e16b71d0dd64efdeb4c0a5e385ad664800a24a Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 25 Jan 2023 13:10:55 -0500 Subject: [PATCH 10/15] Version bump to 1.3.1 --- flatland/package.xml | 2 +- flatland_msgs/package.xml | 2 +- flatland_plugins/package.xml | 2 +- flatland_server/package.xml | 2 +- flatland_viz/package.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/flatland/package.xml b/flatland/package.xml index 6887d27e..ca63c391 100644 --- a/flatland/package.xml +++ b/flatland/package.xml @@ -1,7 +1,7 @@ flatland - 1.3.0 + 1.3.1 This is the metapackage for flatland. diff --git a/flatland_msgs/package.xml b/flatland_msgs/package.xml index 0da348bf..52f18f37 100644 --- a/flatland_msgs/package.xml +++ b/flatland_msgs/package.xml @@ -1,7 +1,7 @@ flatland_msgs - 1.3.0 + 1.3.1 The flatland_msgs package BSD https://bitbucket.org/avidbots/flatland diff --git a/flatland_plugins/package.xml b/flatland_plugins/package.xml index 4f01d918..24ecea33 100644 --- a/flatland_plugins/package.xml +++ b/flatland_plugins/package.xml @@ -1,7 +1,7 @@ flatland_plugins - 1.3.0 + 1.3.1 Default plugins for flatland BSD https://bitbucket.org/avidbots/flatland diff --git a/flatland_server/package.xml b/flatland_server/package.xml index 778f95f7..a862664c 100644 --- a/flatland_server/package.xml +++ b/flatland_server/package.xml @@ -1,7 +1,7 @@ flatland_server - 1.3.0 + 1.3.1 The flatland_server package BSD https://bitbucket.org/avidbots/flatland diff --git a/flatland_viz/package.xml b/flatland_viz/package.xml index a0a225ae..9c8b24f9 100644 --- a/flatland_viz/package.xml +++ b/flatland_viz/package.xml @@ -1,7 +1,7 @@ flatland_viz - 1.3.0 + 1.3.1 The flatland gui and visualization BSD https://bitbucket.org/avidbots/flatland From ddfdbf2bbc46039f4fc1b67a0dea0c6d5490d156 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 25 Jan 2023 13:32:35 -0500 Subject: [PATCH 11/15] fixed lidar scan data direction, which apparently was backwards forever --- docs/included_plugins/laser.rst | 8 ++- .../include/flatland_plugins/laser.h | 2 +- flatland_plugins/src/laser.cpp | 64 ++++++++----------- flatland_plugins/test/laser_test.cpp | 54 +++++++++++++--- .../robot.model.cw.asymmetrical.yaml | 34 ++++++++++ .../range_test/robot.model.cw.yaml | 2 +- .../range_test/world.cw.asymmetrical.yaml | 13 ++++ flatland_server/launch/server.launch | 2 +- 8 files changed, 129 insertions(+), 50 deletions(-) create mode 100644 flatland_plugins/test/laser_tests/range_test/robot.model.cw.asymmetrical.yaml create mode 100644 flatland_plugins/test/laser_tests/range_test/world.cw.asymmetrical.yaml diff --git a/docs/included_plugins/laser.rst b/docs/included_plugins/laser.rst index 92cf7978..ab847ea7 100644 --- a/docs/included_plugins/laser.rst +++ b/docs/included_plugins/laser.rst @@ -55,9 +55,11 @@ messages. # lasers only detects objects in the specified layers layers: ["all"] - # optional, set the scan direction of the lidar sweep - # "clockwise" or "counter-clockwise", defaults to "counter-clockwise" - scan_direction: "counter-clockwise" + # optional, invert the mounting orientation of the lidar (default: false) + # This will invert the laser's body->laser frame TF (roll=PI) + # And sweep the lidar scan across a field mirrored across its mounted axis + # (as if you physically mounted the lidar upside down) + upside_down: false # another example diff --git a/flatland_plugins/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h index 6088fdcb..5f51da79 100644 --- a/flatland_plugins/include/flatland_plugins/laser.h +++ b/flatland_plugins/include/flatland_plugins/laser.h @@ -81,7 +81,7 @@ class Laser : public ModelPlugin { 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 scan_clockwise_; ///< whether the lidar is scan_direction + bool upside_down_; ///< whether the lidar is mounted upside down uint16_t layers_bits_; ///< for setting the layers where laser will function ThreadPool pool_; ///< ThreadPool for managing concurrent scan threads diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index 749f17ee..3fdb5473 100644 --- a/flatland_plugins/src/laser.cpp +++ b/flatland_plugins/src/laser.cpp @@ -84,10 +84,15 @@ void Laser::OnInitialize(const YAML::Node& config) { // pre-calculate the laser points w.r.t to the laser frame, since this never // changes for (unsigned int i = 0; i < num_laser_points; i++) { + float angle = min_angle_ + i * increment_; + if (upside_down_) { // Laser inverted, so laser local frame angles are also inverted + angle = -angle; + } - float x = range_ * cos(angle); - float y = range_ * sin(angle); + // Negative because box2d uses inverted y axis + float x = range_ * cos(-angle); + float y = range_ * sin(-angle); m_laser_points_(0, i) = x; m_laser_points_(1, i) = y; @@ -113,7 +118,12 @@ void Laser::OnInitialize(const YAML::Node& config) { // Broadcast transform between the body and laser tf::Quaternion q; - q.setRPY(0, 0, origin_.theta); + if (upside_down_) { + q.setRPY(0, 0, origin_.theta); + } else { + q.setRPY(M_PI, 0, origin_.theta); + } + laser_tf_.header.frame_id = tf::resolve( "", GetModel()->NameSpaceTF(body_->GetName())); // Todo: parent_tf param @@ -239,37 +249,19 @@ void Laser::ComputeLaserRanges() { // Unqueue all of the future'd results const auto reflectance = reflectance_layers_bits_; - if (scan_clockwise_) { - 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; - } + 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; } } } @@ -307,7 +299,7 @@ void Laser::ParseParameters(const YAML::Node& config) { range_ = reader.Get("range"); noise_std_dev_ = reader.Get("noise_std_dev", 0); - scan_clockwise_ = reader.Get("scan_direction", "counter-clockwise").compare("clockwise") ? false : true; + upside_down_ = reader.Get("upside_down", false); std::vector layers = reader.GetList("layers", {"all"}, -1, -1); @@ -345,7 +337,7 @@ void Laser::ParseParameters(const YAML::Node& config) { rng_ = std::default_random_engine(rd()); noise_gen_ = std::normal_distribution(0.0, noise_std_dev_); - ROS_DEBUG_NAMED("LaserPlugin", + ROS_INFO(//"LaserPlugin", "Laser %s params: topic(%s) body(%s, %p) origin(%f,%f,%f) " "frame_id(%s) broadcast_tf(%d) update_rate(%f) range(%f) " "noise_std_dev(%f) angle_min(%f) angle_max(%f) " diff --git a/flatland_plugins/test/laser_test.cpp b/flatland_plugins/test/laser_test.cpp index e3163e0e..75cb1ca4 100644 --- a/flatland_plugins/test/laser_test.cpp +++ b/flatland_plugins/test/laser_test.cpp @@ -193,18 +193,18 @@ TEST_F(LaserPluginTest, range_test) { // 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.0, 0.0, 5.0, {4.3, 4.4, 4.5}, {})); 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.0, 0.0, 5.0, {4.8, 4.9, 4.6, 4.7, 4.8}, {})); 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, 4, {NAN, NAN, 3.5, 3.2, NAN}, {})); EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p2->update_rate_; EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]); } @@ -237,12 +237,50 @@ TEST_F(LaserPluginTest, scan_direction_test) { // 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.3, 4.4, 4.5}, {})); + 0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3}, {})); EXPECT_TRUE(fltcmp(p1->update_rate_, std::numeric_limits::infinity())) << "Actual: " << p1->update_rate_; EXPECT_EQ(p1->body_, w->models_[0]->bodies_[0]); } +/** + * Test the laser plugin's ability to flip the scan direction to clockwise works as expected + */ +TEST_F(LaserPluginTest, scan_direction_test2) { + world_yaml = this_file_dir / fs::path("laser_tests/range_test/world.cw.asymmetrical.yaml"); + + Timekeeper timekeeper; + timekeeper.SetMaxStepSize(1.0); + w = World::MakeWorld(world_yaml.string()); + + ros::NodeHandle nh; + ros::Subscriber sub_1, sub_2; + LaserPluginTest* obj = dynamic_cast(this); + sub_1 = nh.subscribe("r/scan1", 1, &LaserPluginTest::ScanFrontCb, obj); + sub_2 = nh.subscribe("r/scan2", 1, &LaserPluginTest::ScanCenterCb, obj); + + + Laser* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + Laser* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); + + // let it spin for 10 times to make sure the message gets through + ros::WallRate rate(500); + for (unsigned int i = 0; i < 10; i++) { + w->Update(timekeeper); + ros::spinOnce(); + rate.sleep(); + } + + // TODO: See if this is getting the message :/ + + // check scan returns + EXPECT_TRUE(ScanEq(scan_front, "r_laser_flipped_custom", 0, 0.21, 0.1, + 0.0, 0.0, 0.0, 5.0, {4.489490,4.605707,4.777099}, {})); + EXPECT_TRUE(ScanEq(scan_center, "r_laser_normal", 0, 0.21, 0.1, + 0.0, 0.0, 0.0, 5.0, {4.489490,4.422091,4.400000}, {})); +} + + /** * Test the laser plugin for intensity configuration */ @@ -275,17 +313,17 @@ TEST_F(LaserPluginTest, intensity_test) { // 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})); + 0.0, 0.0, 0.0, 5.0, {4.3, 4.4, 4.5}, {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})); + 0.0, 0.0, 5.0, {4.8, 4.9, 4.6, 4.7, 4.8}, + {0, 0, 0, 255, 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})); + 0.0, 4, {NAN, NAN, 3.5, 3.2, 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]); } diff --git a/flatland_plugins/test/laser_tests/range_test/robot.model.cw.asymmetrical.yaml b/flatland_plugins/test/laser_tests/range_test/robot.model.cw.asymmetrical.yaml new file mode 100644 index 00000000..5c634bb0 --- /dev/null +++ b/flatland_plugins/test/laser_tests/range_test/robot.model.cw.asymmetrical.yaml @@ -0,0 +1,34 @@ +# Turtlebot + +bodies: # List of named bodies + - name: base_link + pose: [0, 0, 0] + type: dynamic + color: [1, 1, 0, 1] + footprints: + - type: polygon + density: 1.0 + points: [[-.5, -0.25], [-.5, 0.25], [.5, 0.25], [.5, -0.25]] + +plugins: + - type: Laser + name: laser_flipped + body: base_link + range: 5 + topic: scan1 + frame: laser_flipped_custom + upside_down: true + angle: {min: 0, max: 0.21, increment: 0.1} + - type: Laser + name: laser_normal + body: base_link + range: 5 + topic: scan2 + upside_down: false + angle: {min: 0, max: 0.21, increment: 0.1} + - type: ModelTfPublisher + name: state_publisher + update_rate: .inf + publish_tf_world: true + world_frame_id: map + reference: base_link \ No newline at end of file diff --git a/flatland_plugins/test/laser_tests/range_test/robot.model.cw.yaml b/flatland_plugins/test/laser_tests/range_test/robot.model.cw.yaml index d24b22e0..318f8df1 100644 --- a/flatland_plugins/test/laser_tests/range_test/robot.model.cw.yaml +++ b/flatland_plugins/test/laser_tests/range_test/robot.model.cw.yaml @@ -16,5 +16,5 @@ plugins: name: laser_front body: base_link range: 5 - scan_direction: clockwise + upside_down: true angle: {min: -1.5707963267948966, max: 1.5707963267948966, increment: 1.5707963267948966} diff --git a/flatland_plugins/test/laser_tests/range_test/world.cw.asymmetrical.yaml b/flatland_plugins/test/laser_tests/range_test/world.cw.asymmetrical.yaml new file mode 100644 index 00000000..9a20cd8a --- /dev/null +++ b/flatland_plugins/test/laser_tests/range_test/world.cw.asymmetrical.yaml @@ -0,0 +1,13 @@ +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] +models: + - name: robot1 + pose: [5, 5, 0.2] + model: robot.model.cw.asymmetrical.yaml + namespace: "r" diff --git a/flatland_server/launch/server.launch b/flatland_server/launch/server.launch index a465b1c7..822326b1 100644 --- a/flatland_server/launch/server.launch +++ b/flatland_server/launch/server.launch @@ -4,7 +4,7 @@ You can override these default values: roslaunch flatland_Server server.launch world_path:="/some/world.yaml" initial_rate:="30.0" --> - + From d947f97d7a9fe6a0b0dc7dcc719ae4e82aa91364 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 25 Jan 2023 14:26:56 -0500 Subject: [PATCH 12/15] fixed incorrect lidar flip code --- flatland_plugins/src/laser.cpp | 9 ++++----- flatland_plugins/test/laser_test.cpp | 20 ++++++++++---------- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index 3fdb5473..e6ecc866 100644 --- a/flatland_plugins/src/laser.cpp +++ b/flatland_plugins/src/laser.cpp @@ -90,9 +90,8 @@ void Laser::OnInitialize(const YAML::Node& config) { angle = -angle; } - // Negative because box2d uses inverted y axis - float x = range_ * cos(-angle); - float y = range_ * sin(-angle); + float x = range_ * cos(angle); + float y = range_ * sin(angle); m_laser_points_(0, i) = x; m_laser_points_(1, i) = y; @@ -119,9 +118,9 @@ void Laser::OnInitialize(const YAML::Node& config) { // Broadcast transform between the body and laser tf::Quaternion q; if (upside_down_) { - q.setRPY(0, 0, origin_.theta); - } else { q.setRPY(M_PI, 0, origin_.theta); + } else { + q.setRPY(0, 0, origin_.theta); } diff --git a/flatland_plugins/test/laser_test.cpp b/flatland_plugins/test/laser_test.cpp index 75cb1ca4..1bc92049 100644 --- a/flatland_plugins/test/laser_test.cpp +++ b/flatland_plugins/test/laser_test.cpp @@ -193,18 +193,18 @@ TEST_F(LaserPluginTest, range_test) { // 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.3, 4.4, 4.5}, {})); + 0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3}, {})); 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.9, 4.6, 4.7, 4.8}, {})); + 0.0, 0.0, 5.0, {4.8, 4.7, 4.6, 4.9, 4.8}, {})); 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, NAN, 3.5, 3.2, NAN}, {})); + 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, {})); EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p2->update_rate_; EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]); } @@ -237,7 +237,7 @@ TEST_F(LaserPluginTest, scan_direction_test) { // 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.0, 0.0, 5.0, {4.3, 4.4, 4.5}, {})); EXPECT_TRUE(fltcmp(p1->update_rate_, std::numeric_limits::infinity())) << "Actual: " << p1->update_rate_; EXPECT_EQ(p1->body_, w->models_[0]->bodies_[0]); @@ -275,9 +275,9 @@ TEST_F(LaserPluginTest, scan_direction_test2) { // check scan returns EXPECT_TRUE(ScanEq(scan_front, "r_laser_flipped_custom", 0, 0.21, 0.1, - 0.0, 0.0, 0.0, 5.0, {4.489490,4.605707,4.777099}, {})); - EXPECT_TRUE(ScanEq(scan_center, "r_laser_normal", 0, 0.21, 0.1, 0.0, 0.0, 0.0, 5.0, {4.489490,4.422091,4.400000}, {})); + EXPECT_TRUE(ScanEq(scan_center, "r_laser_normal", 0, 0.21, 0.1, + 0.0, 0.0, 0.0, 5.0, {4.489490,4.605707,4.777099}, {})); } @@ -313,17 +313,17 @@ TEST_F(LaserPluginTest, intensity_test) { // 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.3, 4.4, 4.5}, {0, 0, 0})); + 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.9, 4.6, 4.7, 4.8}, - {0, 0, 0, 255, 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, NAN, 3.5, 3.2, NAN}, {0, 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]); } From d0acfbe1f440113e4849ee81c8678ddf81ffc5c3 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 25 Jan 2023 14:39:34 -0500 Subject: [PATCH 13/15] reverted unintentional launchfile change --- flatland_server/launch/server.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flatland_server/launch/server.launch b/flatland_server/launch/server.launch index 822326b1..a465b1c7 100644 --- a/flatland_server/launch/server.launch +++ b/flatland_server/launch/server.launch @@ -4,7 +4,7 @@ You can override these default values: roslaunch flatland_Server server.launch world_path:="/some/world.yaml" initial_rate:="30.0" --> - + From 60e4b528fc48c6cd894e9fe2e3f7a0b6ebd49b69 Mon Sep 17 00:00:00 2001 From: Paul Belanger Date: Wed, 25 Jan 2023 18:32:07 -0500 Subject: [PATCH 14/15] laser plugin: support clockwise-turning lidars * Cases where angle_max < angle_min are now allowed when increment < 0 * Added 'upside_down' flag status to diagnostic message --- flatland_plugins/src/laser.cpp | 35 ++++++++++++++++++++++------------ 1 file changed, 23 insertions(+), 12 deletions(-) diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index e6ecc866..b6b4b1c3 100644 --- a/flatland_plugins/src/laser.cpp +++ b/flatland_plugins/src/laser.cpp @@ -311,8 +311,20 @@ void Laser::ParseParameters(const YAML::Node& config) { angle_reader.EnsureAccessedAllKeys(); reader.EnsureAccessedAllKeys(); - if (max_angle_ < min_angle_) { - throw YAMLException("Invalid \"angle\" params, must have max > min"); + if (increment_ < 0) { + if (min_angle_ < max_angle_) { + throw YAMLException( + "Invalid \"angle\" params, must have min > max when increment < 0"); + } + + } else if (increment_ > 0) { + if (max_angle_ < min_angle_) { + throw YAMLException( + "Invalid \"angle\" params, must have max > min when increment > 0"); + } + } else { + throw YAMLException( + "Invalid \"angle\" params, increment must not be zero!"); } body_ = GetModel()->GetBody(body_name); @@ -336,16 +348,15 @@ void Laser::ParseParameters(const YAML::Node& config) { rng_ = std::default_random_engine(rd()); noise_gen_ = std::normal_distribution(0.0, noise_std_dev_); - ROS_INFO(//"LaserPlugin", - "Laser %s params: topic(%s) body(%s, %p) origin(%f,%f,%f) " - "frame_id(%s) broadcast_tf(%d) update_rate(%f) range(%f) " - "noise_std_dev(%f) angle_min(%f) angle_max(%f) " - "angle_increment(%f) layers(0x%u {%s})", - GetName().c_str(), topic_.c_str(), body_name.c_str(), body_, - origin_.x, origin_.y, origin_.theta, frame_id_.c_str(), - broadcast_tf_, update_rate_, range_, noise_std_dev_, - min_angle_, max_angle_, increment_, layers_bits_, - boost::algorithm::join(layers, ",").c_str()); + ROS_INFO( //"LaserPlugin", + "Laser %s params: topic(%s) body(%s, %p) origin(%f,%f,%f) upside_down(%d)" + "frame_id(%s) broadcast_tf(%d) update_rate(%f) range(%f) " + "noise_std_dev(%f) angle_min(%f) angle_max(%f) " + "angle_increment(%f) layers(0x%u {%s})", + GetName().c_str(), topic_.c_str(), body_name.c_str(), body_, origin_.x, + origin_.y, origin_.theta, upside_down_, frame_id_.c_str(), broadcast_tf_, + update_rate_, range_, noise_std_dev_, min_angle_, max_angle_, increment_, + layers_bits_, boost::algorithm::join(layers, ",").c_str()); } }; // namespace flatland_plugins From 713a8fe151f2fdbd452c6c19ee976f0177f8b63a Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 26 Jan 2023 16:16:48 -0500 Subject: [PATCH 15/15] Update laser.rst --- docs/included_plugins/laser.rst | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docs/included_plugins/laser.rst b/docs/included_plugins/laser.rst index ab847ea7..d6c3dd2a 100644 --- a/docs/included_plugins/laser.rst +++ b/docs/included_plugins/laser.rst @@ -46,6 +46,9 @@ messages. # required, w.r.t to the coordinate system, scan from min angle to max angle # at steps of specified increments + # Scan direction defaults to counter-clockwise but clockwise rotations can be + # simulated by providing a decrementing angle configuration. + # e.g. min: 2, max: -2, increment: -0.1 (clockwise) angle: {min: -2.356194490192345, max: 2.356194490192345, increment: 0.004363323129985824} # optional, default to inf (as fast as possible), rate to publish laser scan messages