From 6eda9324b2527bc4d67ca33e39eaf1964da98fd0 Mon Sep 17 00:00:00 2001 From: joseph Date: Fri, 10 Sep 2021 12:17:58 -0400 Subject: [PATCH 1/6] Forced RelWithDebInfo for flatland_server and flatland_plugins. Simulation performance suffers a lot without optimizations enabled --- flatland_plugins/CMakeLists.txt | 1 + flatland_server/CMakeLists.txt | 2 ++ 2 files changed, 3 insertions(+) diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index edb26ac5..cf35482d 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -3,6 +3,7 @@ project(flatland_plugins) # Ensure we're using c++11 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +set(CMAKE_BUILD_TYPE RelWithDebInfo) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 59711c12..37675f1b 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -4,6 +4,8 @@ project(flatland_server) # Ensure we're using c++11 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +set(CMAKE_BUILD_TYPE RelWithDebInfo) + ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages From b80d5cb49ff0dd92cd8ec1e6a5292fd83aa94f40 Mon Sep 17 00:00:00 2001 From: joseph Date: Fri, 10 Sep 2021 12:18:54 -0400 Subject: [PATCH 2/6] Fixed incorrect logic on legacy tricycle_drive parameter loading --- flatland_plugins/src/tricycle_drive.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flatland_plugins/src/tricycle_drive.cpp b/flatland_plugins/src/tricycle_drive.cpp index 426325fc..266a3f6e 100644 --- a/flatland_plugins/src/tricycle_drive.cpp +++ b/flatland_plugins/src/tricycle_drive.cpp @@ -105,8 +105,8 @@ void TricycleDrive::OnInitialize(const YAML::Node& config) { angular_dynamics_.Configure(r.SubnodeOpt("angular_dynamics", YamlReader::MAP).Node()); // Accept old configuration location for angular dynamics constraints if present - if (angular_dynamics_.velocity_limit_ != 0.0) angular_dynamics_.velocity_limit_ = r.Get("max_angular_velocity", 0.0); - if (angular_dynamics_.acceleration_limit_ != 0.0) { + if (angular_dynamics_.velocity_limit_ == 0.0) angular_dynamics_.velocity_limit_ = r.Get("max_angular_velocity", 0.0); + if (angular_dynamics_.acceleration_limit_ == 0.0) { angular_dynamics_.acceleration_limit_ = r.Get("max_steer_acceleration", 0.0); angular_dynamics_.deceleration_limit_ = angular_dynamics_.acceleration_limit_ ; } From 26ce985bf3fa4a80590bfbadc75d676ab6518148 Mon Sep 17 00:00:00 2001 From: joseph Date: Fri, 10 Sep 2021 12:31:52 -0400 Subject: [PATCH 3/6] Cherry picked Rapyuta robotics smart optimization to the laser scan plugin from their rr-devl branch --- .../include/flatland_plugins/laser.h | 37 +++-- flatland_plugins/src/laser.cpp | 154 +++++++++++++----- .../thirdparty/Box2D/Dynamics/b2World.cpp | 4 +- .../thirdparty/Box2D/Dynamics/b2World.h | 2 +- 4 files changed, 139 insertions(+), 58 deletions(-) diff --git a/flatland_plugins/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h index 68d83452..6e8aff04 100644 --- a/flatland_plugins/include/flatland_plugins/laser.h +++ b/flatland_plugins/include/flatland_plugins/laser.h @@ -73,14 +73,15 @@ class Laser : public ModelPlugin { std::string topic_; ///< topic name to publish the laser scan Body *body_; ///< body the laser frame attaches to Pose origin_; ///< laser frame w.r.t the body - double range_; ///< laser max range - double noise_std_dev_; ///< noise std deviation - double max_angle_; /// < laser max angle - double min_angle_; ///< laser min angle - double increment_; ///< laser angle increment - double update_rate_; ///< the rate laser scan will be published + float range_; ///< laser max range + float noise_std_dev_; ///< noise std deviation + float max_angle_; /// < laser max angle + float min_angle_; ///< laser min angle + float increment_; ///< laser angle increment + 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 uint16_t layers_bits_; ///< for setting the layers where laser will function ThreadPool pool_; ///< ThreadPool for managing concurrent scan threads @@ -90,17 +91,19 @@ class Laser : public ModelPlugin { */ uint16_t reflectance_layers_bits_; - std::default_random_engine rng_; ///< random generator - std::normal_distribution noise_gen_; ///< gaussian noise generator - - Eigen::Matrix3f m_body_to_laser_; ///< tf from body to laser - Eigen::Matrix3f m_world_to_body_; ///< tf from world to body - Eigen::Matrix3f m_world_to_laser_; ///< tf from world to laser - Eigen::MatrixXf m_laser_points_; ///< laser points in the laser' frame - Eigen::MatrixXf m_world_laser_points_; /// laser point in the world frame - 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 + std::default_random_engine rng_; ///< random generator + std::normal_distribution noise_gen_; ///< gaussian noise generator + + Eigen::Matrix3f m_body_to_laser_; ///< tf from body to laser + Eigen::Matrix3f m_world_to_body_; ///< tf from world to body + Eigen::Matrix3f m_world_to_laser_; ///< tf from world to laser + Eigen::MatrixXf m_laser_points_; ///< laser points in the laser' frame + Eigen::MatrixXf m_world_laser_points_; /// laser point in the world frame + 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 + std::vector m_lastMaxFractions_; ///< the robot move slowly when + /// comparing with to the scan rate 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 318d3133..b210c538 100644 --- a/flatland_plugins/src/laser.cpp +++ b/flatland_plugins/src/laser.cpp @@ -59,7 +59,7 @@ using namespace flatland_server; namespace flatland_plugins { -void Laser::OnInitialize(const YAML::Node &config) { +void Laser::OnInitialize(const YAML::Node& config) { ParseParameters(config); update_timer_.SetRate(update_rate_); @@ -78,6 +78,7 @@ void Laser::OnInitialize(const YAML::Node &config) { // initialize size for the matrix storing the laser points m_laser_points_ = Eigen::MatrixXf(3, num_laser_points); m_world_laser_points_ = Eigen::MatrixXf(3, num_laser_points); + m_lastMaxFractions_ = std::vector(num_laser_points, 1.0); v_zero_point_ << 0, 0, 1; // pre-calculate the laser points w.r.t to the laser frame, since this never @@ -107,8 +108,7 @@ void Laser::OnInitialize(const YAML::Node &config) { else laser_scan_.intensities.resize(0); laser_scan_.header.seq = 0; - laser_scan_.header.frame_id = - tf::resolve("", GetModel()->NameSpaceTF(frame_id_)); + laser_scan_.header.frame_id = frame_id_; // Broadcast transform between the body and laser tf::Quaternion q; @@ -127,7 +127,7 @@ void Laser::OnInitialize(const YAML::Node &config) { laser_tf_.transform.rotation.w = q.w(); } -void Laser::BeforePhysicsStep(const Timekeeper &timekeeper) { +void Laser::BeforePhysicsStep(const Timekeeper& timekeeper) { // keep the update rate if (!update_timer_.CheckUpdate(timekeeper)) { return; @@ -135,7 +135,9 @@ void Laser::BeforePhysicsStep(const Timekeeper &timekeeper) { // only compute and publish when the number of subscribers is not zero if (scan_publisher_.getNumSubscribers() > 0) { + //START_PROFILE(timekeeper, "compute laser range"); ComputeLaserRanges(); + //END_PROFILE(timekeeper, "compute laser range"); laser_scan_.header.stamp = timekeeper.GetSimTime(); scan_publisher_.publish(laser_scan_); } @@ -150,7 +152,7 @@ void Laser::ComputeLaserRanges() { // get the transformation matrix from the world to the body, and get the // world to laser frame transformation matrix by multiplying the world to body // and body to laser - const b2Transform &t = body_->GetPhysicsBody()->GetTransform(); + const b2Transform& t = body_->GetPhysicsBody()->GetTransform(); m_world_to_body_ << t.q.c, -t.q.s, t.p.x, t.q.s, t.q.c, t.p.y, 0, 0, 1; m_world_to_laser_ = m_world_to_body_ * m_body_to_laser_; @@ -163,42 +165,116 @@ void Laser::ComputeLaserRanges() { // Conver to Box2D data types b2Vec2 laser_origin_point(v_world_laser_origin_(0), v_world_laser_origin_(1)); - // Results vector - std::vector>> results( - laser_scan_.ranges.size()); - - // loop through the laser points and call the Box2D world raycast by - // enqueueing the callback - for (unsigned int i = 0; i < laser_scan_.ranges.size(); ++i) { - results[i] = - pool_.enqueue([i, this, laser_origin_point] { // Lambda function - b2Vec2 laser_point; - laser_point.x = m_world_laser_points_(0, i); - laser_point.y = m_world_laser_points_(1, i); - LaserCallback cb(this); - - GetModel()->GetPhysicsWorld()->RayCast(&cb, laser_origin_point, - laser_point); - - if (!cb.did_hit_) { - return std::make_pair(NAN, 0); - } else { - return std::make_pair(cb.fraction_ * this->range_, - cb.intensity_); - } - }); + // Results vector : clustered to avoid too many locks + constexpr size_t clusterSize = 10; + // The last cluster may have a different size + size_t lastClusterSize = laser_scan_.ranges.size() % clusterSize; + size_t nbCluster = + laser_scan_.ranges.size() / clusterSize + (lastClusterSize == 0 ? 0 : 1); + if (lastClusterSize == 0) { + lastClusterSize = clusterSize; + } + std::vector>>> results( + nbCluster); + + // At scan n : we hit an obstacle at distance range_ * coef + // At scan n+1 : we try a first scan with length = range_ * coef * + // lastScanExpansion + // lastScanExpansion makes it possible to take into account of the motion + // and the rotation of the laser + const float lastScanExpansion = + 1.0f + std::max(4.0f / (range_ * update_rate_), 0.05f); + + // loop through the laser points and call the Box2D world raycast + // by enqueueing the callback + for (unsigned int i = 0; i < nbCluster; ++i) { + size_t currentClusterSize = + (i == nbCluster - 1 ? lastClusterSize : clusterSize); + results[i] = pool_.enqueue([i, currentClusterSize, clusterSize, this, + laser_origin_point, lastScanExpansion] { + std::vector> out(currentClusterSize); + size_t currentIndice = i * clusterSize; + // Iterating over the currentClusterSize indices, starting at i * + // clusterSize + for (size_t j = 0; j < currentClusterSize; ++j, ++currentIndice) { + b2Vec2 laser_point; + laser_point.x = m_world_laser_points_(0, currentIndice); + laser_point.y = m_world_laser_points_(1, currentIndice); + LaserCallback cb(this); + + // 1 - Take advantage of the last scan : we may hit the same obstacle + // (if any), + // with the first part of the ray being [0 fraction] * range_ + float fraction = std::min( + lastScanExpansion * m_lastMaxFractions_[currentIndice], 1.0f); + GetModel()->GetPhysicsWorld()->RayCast(&cb, laser_origin_point, + laser_point, fraction); + + // 2 - If no hit detected, we relaunch the scan + // with the second part of the ray being [fraction 1] * range_ + if (!cb.did_hit_ && fraction < 1.0f) { + b2Vec2 new_origin_point = + fraction * laser_point + (1 - fraction) * laser_origin_point; + GetModel()->GetPhysicsWorld()->RayCast(&cb, new_origin_point, + laser_point, 1.0f); + if (cb.did_hit_) + cb.fraction_ = fraction + cb.fraction_ - cb.fraction_ * fraction; + } + + // 3 - Let's check the result + if (cb.did_hit_) { + m_lastMaxFractions_[currentIndice] = cb.fraction_; + out[j] = std::make_pair( + cb.fraction_ * this->range_ + this->noise_gen_(this->rng_), + static_cast(cb.intensity_)); + } else { + m_lastMaxFractions_[currentIndice] = cb.fraction_; + out[j] = std::make_pair(NAN, 0); + } + } + return out; + }); } // Unqueue all of the future'd results - for (unsigned int i = 0; i < laser_scan_.ranges.size(); ++i) { - auto result = results[i].get(); // Pull the result from the future - laser_scan_.ranges[i] = result.first + this->noise_gen_(this->rng_); - if (reflectance_layers_bits_) laser_scan_.intensities[i] = result.second; + 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; + } + } } } -float LaserCallback::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, - const b2Vec2 &normal, float fraction) { +float LaserCallback::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 (!(category_bits & parent_->layers_bits_)) { @@ -218,7 +294,7 @@ float LaserCallback::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, return fraction; } -void Laser::ParseParameters(const YAML::Node &config) { +void Laser::ParseParameters(const YAML::Node& config) { YamlReader reader(config); std::string body_name = reader.Get("body"); topic_ = reader.Get("topic", "scan"); @@ -230,6 +306,8 @@ 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); + std::vector layers = reader.GetList("layers", {"all"}, -1, -1); @@ -264,7 +342,7 @@ void Laser::ParseParameters(const YAML::Node &config) { // init the random number generators std::random_device rd; rng_ = std::default_random_engine(rd()); - noise_gen_ = std::normal_distribution(0.0, noise_std_dev_); + 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) " @@ -277,6 +355,6 @@ void Laser::ParseParameters(const YAML::Node &config) { min_angle_, max_angle_, increment_, layers_bits_, boost::algorithm::join(layers, ",").c_str()); } -}; +}; // namespace flatland_plugins PLUGINLIB_EXPORT_CLASS(flatland_plugins::Laser, flatland_server::ModelPlugin) diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2World.cpp b/flatland_server/thirdparty/Box2D/Dynamics/b2World.cpp index 201d5160..60a125a5 100644 --- a/flatland_server/thirdparty/Box2D/Dynamics/b2World.cpp +++ b/flatland_server/thirdparty/Box2D/Dynamics/b2World.cpp @@ -1016,13 +1016,13 @@ struct b2WorldRayCastWrapper b2RayCastCallback* callback; }; -void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const +void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2, float maxFraction/*=1.0*/) const { b2WorldRayCastWrapper wrapper; wrapper.broadPhase = &m_contactManager.m_broadPhase; wrapper.callback = callback; b2RayCastInput input; - input.maxFraction = 1.0f; + input.maxFraction = maxFraction; input.p1 = point1; input.p2 = point2; m_contactManager.m_broadPhase.RayCast(&wrapper, input); diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2World.h b/flatland_server/thirdparty/Box2D/Dynamics/b2World.h index fa0ba0b8..460b9310 100644 --- a/flatland_server/thirdparty/Box2D/Dynamics/b2World.h +++ b/flatland_server/thirdparty/Box2D/Dynamics/b2World.h @@ -119,7 +119,7 @@ class b2World /// @param callback a user implemented callback class. /// @param point1 the ray starting point /// @param point2 the ray ending point - void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const; + void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2, float maxFraction=1.0f) const; /// Get the world body list. With the returned body, use b2Body::GetNext to get /// the next body in the world list. A nullptr body indicates the end of the list. From 113af7cb877e6c6aa15c588df59be7a908fcacc9 Mon Sep 17 00:00:00 2001 From: joseph Date: Fri, 10 Sep 2021 13:08:44 -0400 Subject: [PATCH 4/6] Moved map_to_lines.py into flatland_server program install space --- flatland_server/CMakeLists.txt | 4 ++++ {scripts => flatland_server/scripts}/map_to_lines.py | 0 2 files changed, 4 insertions(+) rename {scripts => flatland_server/scripts}/map_to_lines.py (100%) diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 37675f1b..044ee8c6 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -178,6 +178,10 @@ install(FILES flatland_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) +install(PROGRAMS scripts/map_to_lines.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + ############# ## Testing ## ############# diff --git a/scripts/map_to_lines.py b/flatland_server/scripts/map_to_lines.py similarity index 100% rename from scripts/map_to_lines.py rename to flatland_server/scripts/map_to_lines.py From 60485c84ea1dedcf3845fb0a43c4349172cfce44 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Tue, 3 May 2022 17:00:31 -0400 Subject: [PATCH 5/6] removed changes to tf namespacing --- flatland_plugins/src/laser.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index b210c538..b60d1f75 100644 --- a/flatland_plugins/src/laser.cpp +++ b/flatland_plugins/src/laser.cpp @@ -108,7 +108,8 @@ void Laser::OnInitialize(const YAML::Node& config) { else laser_scan_.intensities.resize(0); laser_scan_.header.seq = 0; - laser_scan_.header.frame_id = frame_id_; + laser_scan_.header.frame_id = + tf::resolve("", GetModel()->NameSpaceTF(frame_id_)); // Broadcast transform between the body and laser tf::Quaternion q; From 5ea5b734d16eccd0d2b0c4240dd53212f391c226 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Tue, 3 May 2022 17:33:08 -0400 Subject: [PATCH 6/6] added warning skip for eigen third party library on clang source check --- scripts/clang_tidy_ignore.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/scripts/clang_tidy_ignore.yaml b/scripts/clang_tidy_ignore.yaml index 9c819842..dfe46473 100644 --- a/scripts/clang_tidy_ignore.yaml +++ b/scripts/clang_tidy_ignore.yaml @@ -3,3 +3,4 @@ - ".*/flatland/flatland_plugins/.*warning: 'log_deprecated' is deprecated \\[clang-diagnostic-deprecated-declarations\\].*" - ".*QtCore/qobject.h:235:16.*warning: Potential memory leak.*\\[clang-analyzer-cplusplus.NewDeleteLeaks\\].*" - ".*/flatland/flatland_viz/.*warning: 'log_deprecated' is deprecated \\[clang-diagnostic-deprecated-declarations\\].*" +- ".*/eigen3/Eigen/.*warning.*"