diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml new file mode 100644 index 00000000..ad06a32a --- /dev/null +++ b/.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}} diff --git a/docs/included_plugins/laser.rst b/docs/included_plugins/laser.rst index d94f6e72..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 @@ -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 - \ No newline at end of file + 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/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h index 6e8aff04..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 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 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_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index b60d1f75..b6b4b1c3 100644 --- a/flatland_plugins/src/laser.cpp +++ b/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("range"); noise_std_dev_ = reader.Get("noise_std_dev", 0); - flipped_ = reader.Get("flipped", false); + upside_down_ = reader.Get("upside_down", false); std::vector layers = reader.GetList("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(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 diff --git a/flatland_plugins/test/laser_test.cpp b/flatland_plugins/test/laser_test.cpp index ba687266..1bc92049 100644 --- a/flatland_plugins/test/laser_test.cpp +++ b/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(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'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.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 */ 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 new file mode 100644 index 00000000..318f8df1 --- /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 + 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_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" 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_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); 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