Merge pull request #94 from avidbots/clean-up-lidar-flip-param

Clean up lidar flip param, and re-introduce CI testing for ros1

.github/workflows/industrial_ci_action.yml

@@ -0,0 +1,15 @@
+name: CI
+
+on: [push, pull_request]
+
+jobs:
+  industrial_ci:
+    strategy:
+      matrix:
+        env:
+          - {ROS_DISTRO: noetic, ROS_REPO: main}
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v1
+      - uses: 'ros-industrial/industrial_ci@master'
+        env: ${{matrix.env}}

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
@@ -55,6 +58,13 @@ messages.
       # lasers only detects objects in the specified layers
       layers: ["all"]
 
+      # 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
     - type: Laser
       name: laser_front
@@ -64,4 +74,4 @@ messages.
       layers: ["layer_1", "layer_2", "layer_3"]
       update_rate: 100
       noise_std_dev: 0.01
-      
+      

flatland/package.xml

@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <package>
   <name>flatland</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>
     This is the metapackage for flatland.
   </description>

flatland_msgs/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flatland_msgs</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>The flatland_msgs package</description>
   <license>BSD</license>
   <url type="website">https://bitbucket.org/avidbots/flatland</url>

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 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
 

flatland_plugins/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flatland_plugins</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>Default plugins for flatland</description>
   <license>BSD</license>
   <url type="website">https://bitbucket.org/avidbots/flatland</url>

flatland_plugins/src/laser.cpp

@@ -84,7 +84,11 @@ 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);
@@ -113,7 +117,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(M_PI, 0, origin_.theta);
+  } else {
+    q.setRPY(0, 0, origin_.theta);
+  }
+
 
   laser_tf_.header.frame_id = tf::resolve(
       "", GetModel()->NameSpaceTF(body_->GetName()));  // Todo: parent_tf param
@@ -239,37 +248,19 @@ void Laser::ComputeLaserRanges() {
 
   // Unqueue all of the future'd results
   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;
-      }
+  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 +298,7 @@ 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);
+  upside_down_ = reader.Get<bool>("upside_down", false);
 
   std::vector<std::string> layers =
       reader.GetList<std::string>("layers", {"all"}, -1, -1);
@@ -320,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);
@@ -345,16 +348,15 @@ void Laser::ParseParameters(const YAML::Node& config) {
   rng_ = std::default_random_engine(rd());
   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) "
-                  "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
 

flatland_plugins/test/laser_test.cpp

@@ -208,6 +208,79 @@ 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<LaserPluginTest*>(this);
+  sub_1 = nh.subscribe("r/scan", 1, &LaserPluginTest::ScanFrontCb, obj);
+
+  Laser* p1 = dynamic_cast<Laser*>(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<float>::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<LaserPluginTest*>(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<Laser*>(w->plugin_manager_.model_plugins_[0].get());
+  Laser* p2 = dynamic_cast<Laser*>(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.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}, {}));
+}
+
+
 /**
  * Test the laser plugin for intensity configuration
  */

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

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
+    upside_down: true
+    angle: {min: -1.5707963267948966, max: 1.5707963267948966, increment: 1.5707963267948966}

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"

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"

flatland_server/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flatland_server</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>The flatland_server package</description>
   <license>BSD</license>
   <url type="website">https://bitbucket.org/avidbots/flatland</url>