From b19877e4322776ad9b1a64e6d74d50a84d93c405 Mon Sep 17 00:00:00 2001
From: Sees <Vsevolod.Egorov@ronavi.ru>
Date: Thu, 14 Mar 2024 14:29:27 +0300
Subject: [PATCH 1/6] added the capability to work with non sim time in
 timekeeper

---
 flatland_server/launch/server.launch |  2 --
 flatland_server/src/timekeeper.cpp   | 13 +++++++++++--
 2 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/flatland_server/launch/server.launch b/flatland_server/launch/server.launch
index a465b1c7..c2651565 100644
--- a/flatland_server/launch/server.launch
+++ b/flatland_server/launch/server.launch
@@ -13,8 +13,6 @@
 
   <env name="ROSCONSOLE_FORMAT" value="[${severity} ${time} ${logger}]: ${message}" />
 
-  <param name="use_sim_time" value="true"/>  
-
   <!-- launch flatland server -->
   <node name="flatland_server" pkg="flatland_server" type="flatland_server" output="screen">  
     <!-- Use the arguments passed into the launchfile for this node -->
diff --git a/flatland_server/src/timekeeper.cpp b/flatland_server/src/timekeeper.cpp
index 8b27b597..77514464 100644
--- a/flatland_server/src/timekeeper.cpp
+++ b/flatland_server/src/timekeeper.cpp
@@ -49,8 +49,17 @@
 
 namespace flatland_server {
 
-Timekeeper::Timekeeper()
-    : time_(ros::Time(0, 0)), max_step_size_(0), clock_topic_("/clock") {
+Timekeeper::Timekeeper() : max_step_size_(0), clock_topic_("/clock") {
+  bool use_sim_time = false;
+  nh_.getParam("use_sim_time", use_sim_time);
+
+  if (!use_sim_time) {
+    time_ =
+        ros::Time(ros::WallTime::now().toSec(), ros::WallTime::now().toNSec());
+  } else {
+    time_ = ros::Time(0, 0);
+  }
+
   clock_pub_ = nh_.advertise<rosgraph_msgs::Clock>(clock_topic_, 1);
 }
 

From 67d87c18e4d39c9b68222ebdfae22262df912a15 Mon Sep 17 00:00:00 2001
From: Joseph Duchesne <joseph@avidbots.com>
Date: Tue, 7 May 2024 16:32:59 -0400
Subject: [PATCH 2/6] Added a lightweight profiling printout on the laser
 plugin

---
 docs/included_plugins/laser.rst               |  3 +++
 .../include/flatland_plugins/laser.h          |  2 ++
 flatland_plugins/src/laser.cpp                | 21 +++++++++++++------
 3 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/docs/included_plugins/laser.rst b/docs/included_plugins/laser.rst
index d6c3dd2a..d600653d 100644
--- a/docs/included_plugins/laser.rst
+++ b/docs/included_plugins/laser.rst
@@ -34,6 +34,9 @@ messages.
       # optional, default to true, whether to publish TF
       broadcast_tf: true
 
+      # optional, default to false, if true compute/publish even if no subscribers (for benchmarking)
+      always_publish: false
+
       # optional, default to name of this plugin, the TF frame id to publish TF with
       # only used when broadcast_tf=true
       frame: laser_back
diff --git a/flatland_plugins/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h
index 5f51da79..7b2ab81e 100644
--- a/flatland_plugins/include/flatland_plugins/laser.h
+++ b/flatland_plugins/include/flatland_plugins/laser.h
@@ -81,9 +81,11 @@ 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 always_publish_;   ///< Force publishing even if no subscribers
   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
+  uint64_t publications_ = 0;
 
   /*
    * for setting reflectance layers. if the laser hits those layers,
diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp
index b6b4b1c3..15e8a7f8 100644
--- a/flatland_plugins/src/laser.cpp
+++ b/flatland_plugins/src/laser.cpp
@@ -54,6 +54,7 @@
 #include <boost/algorithm/string/join.hpp>
 #include <cmath>
 #include <limits>
+#include <chrono>
 
 using namespace flatland_server;
 
@@ -143,13 +144,20 @@ void Laser::BeforePhysicsStep(const Timekeeper& timekeeper) {
     return;
   }
 
-  // only compute and publish when the number of subscribers is not zero
-  if (scan_publisher_.getNumSubscribers() > 0) {
-    //START_PROFILE(timekeeper, "compute laser range");
+  // only compute and publish when the number of subscribers is not zero, or always_publish_ is true
+  if (always_publish_ || scan_publisher_.getNumSubscribers() > 0) {
+    // START_PROFILE(timekeeper, "compute laser range");
+    auto start = std::chrono::steady_clock::now();
     ComputeLaserRanges();
-    //END_PROFILE(timekeeper, "compute laser range");
+
+    ROS_INFO_THROTTLE_NAMED(
+        1, "Laser Plugin", "took %luus",
+        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - start).count());
+
+    // END_PROFILE(timekeeper, "compute laser range");
     laser_scan_.header.stamp = timekeeper.GetSimTime();
     scan_publisher_.publish(laser_scan_);
+    publications_++;
   }
 
   if (broadcast_tf_) {
@@ -292,6 +300,7 @@ void Laser::ParseParameters(const YAML::Node& config) {
   topic_ = reader.Get<std::string>("topic", "scan");
   frame_id_ = reader.Get<std::string>("frame", GetName());
   broadcast_tf_ = reader.Get<bool>("broadcast_tf", true);
+  always_publish_ = reader.Get<bool>("always_publish", false);
   update_rate_ = reader.Get<double>("update_rate",
                                     std::numeric_limits<double>::infinity());
   origin_ = reader.GetPose("origin", Pose(0, 0, 0));
@@ -352,11 +361,11 @@ void Laser::ParseParameters(const YAML::Node& config) {
       "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})",
+      "angle_increment(%f) layers(0x%u {%s}) always_publish(%d)",
       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());
+      layers_bits_, boost::algorithm::join(layers, ",").c_str(), always_publish_);
 }
 };  // namespace flatland_plugins
 

From 238bc034d574b2d0d0e95e44f9a1e59ef74f963c Mon Sep 17 00:00:00 2001
From: Joseph Duchesne <joseph@avidbots.com>
Date: Tue, 7 May 2024 16:37:25 -0400
Subject: [PATCH 3/6] Swapped out vectorization for opencv::findContours, and
 box2d line segments for chain loops

---
 .../include/flatland_server/layer.h           |   3 +-
 flatland_server/src/debug_visualization.cpp   |  21 ++++
 flatland_server/src/layer.cpp                 | 115 +++++++-----------
 3 files changed, 66 insertions(+), 73 deletions(-)

diff --git a/flatland_server/include/flatland_server/layer.h b/flatland_server/include/flatland_server/layer.h
index 137ccee6..8a6727a7 100644
--- a/flatland_server/include/flatland_server/layer.h
+++ b/flatland_server/include/flatland_server/layer.h
@@ -151,9 +151,10 @@ class Layer : public Entity {
    * @param[in] bitmap OpenCV Image
    * @param[in] occupied_thresh Threshold indicating obstacle if above
    * @param[in] resolution Resolution of the map image in meters per pixel
+   * @param[in] simplify Simplification factor: 0=None, 1=moderate, 2=significant
    */
   void LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh,
-                      double resolution);
+                      double resolution, int simplify=0);
 
   /**
    * @brief Visualize layer for debugging purposes
diff --git a/flatland_server/src/debug_visualization.cpp b/flatland_server/src/debug_visualization.cpp
index f29c8e72..ab0001f1 100644
--- a/flatland_server/src/debug_visualization.cpp
+++ b/flatland_server/src/debug_visualization.cpp
@@ -201,6 +201,27 @@ void DebugVisualization::BodyToMarkers(visualization_msgs::MarkerArray& markers,
 
       } break;
 
+      case b2Shape::e_chain: {
+
+        geometry_msgs::Point p;  // b2Edge uses vertex1 and 2 for its edges
+        b2ChainShape* chain = (b2ChainShape*)fixture->GetShape();
+
+        add_marker = true;  
+        marker.type = marker.LINE_STRIP;
+        marker.scale.x = 0.03;  // 3cm wide lines
+        
+        for(int i=0; i<chain->m_count; i++) {
+          p.x = chain->m_vertices[i].x;
+          p.y = chain->m_vertices[i].y;
+          marker.points.push_back(p);
+        }
+
+        // close loop
+        p.x = chain->m_vertices[0].x;
+        p.y = chain->m_vertices[0].y;
+        marker.points.push_back(p);
+      } break;
+
       default:  // Unsupported shape
         ROS_WARN_THROTTLE_NAMED(1.0, "DebugVis", "Unsupported Box2D shape %d",
                                 static_cast<int>(fixture->GetType()));
diff --git a/flatland_server/src/layer.cpp b/flatland_server/src/layer.cpp
index e033dfc2..3c1f515e 100644
--- a/flatland_server/src/layer.cpp
+++ b/flatland_server/src/layer.cpp
@@ -218,98 +218,69 @@ void Layer::ReadLineSegmentsFile(const std::string &file_path,
 }
 
 void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh,
-                           double resolution) {
+                           double resolution, int simplify) {
   uint16_t category_bits = cfr_->GetCategoryBits(names_);
 
-  auto add_edge = [&](double x1, double y1, double x2, double y2) {
-    b2EdgeShape edge;
+  uint32_t edges_added = 0;
+
+  auto add_poly = [&](std::vector<cv::Point2f>& poly) {
+    b2ChainShape polygon_chain;
     double rows = bitmap.rows;
     double res = resolution;
 
-    edge.Set(b2Vec2(res * x1, res * (rows - y1)),
-             b2Vec2(res * x2, res * (rows - y2)));
+    std::vector<b2Vec2> poly_b2;
+    poly_b2.reserve(poly.size());
+    for(auto& p : poly) {
+      poly_b2.emplace_back(res * p.x, res * (rows - p.y));
+    }
+
+    polygon_chain.CreateLoop(&poly_b2.front(), poly_b2.size());
 
     b2FixtureDef fixture_def;
-    fixture_def.shape = &edge;
+    fixture_def.shape = &polygon_chain;
     fixture_def.filter.categoryBits = category_bits;
     fixture_def.filter.maskBits = fixture_def.filter.categoryBits;
     body_->physics_body_->CreateFixture(&fixture_def);
+
+    edges_added += poly.size();
   };
 
   cv::Mat padded_map, obstacle_map;
 
   // thresholds the map, values between the occupied threshold and 1.0 are
   // considered to be occupied
-  cv::inRange(bitmap, occupied_thresh, 1.0, obstacle_map);
-
-  // pad the top and bottom of the map each with an empty row (255=white). This
-  // helps to look at the transition from one row of pixel to another
-  cv::copyMakeBorder(obstacle_map, padded_map, 1, 1, 0, 0, cv::BORDER_CONSTANT,
-                     255);
-
-  // loop through all the rows, looking at 2 at once
-  for (int i = 0; i < padded_map.rows - 1; i++) {
-    cv::Mat row1 = padded_map.row(i);
-    cv::Mat row2 = padded_map.row(i + 1);
-    cv::Mat diff;
-
-    // if the two row are the same value, there is no edge
-    // if the two rows are not the same value, there is an edge
-    // result is still binary, either 255 or 0
-    cv::absdiff(row1, row2, diff);
-
-    int start = 0;
-    bool started = false;
-
-    // find all the walls, put the connected walls as a single line segment
-    for (unsigned int j = 0; j <= diff.total(); j++) {
-      bool edge_exists = false;
-      if (j < diff.total()) {
-        edge_exists = diff.at<uint8_t>(0, j);  // 255 maps to true
-      }
-
-      if (edge_exists && !started) {
-        start = j;
-        started = true;
-      } else if (started && !edge_exists) {
-        add_edge(start, i, j, i);
-
-        started = false;
-      }
-    }
+  cv::inRange(bitmap, occupied_thresh, 1.0, obstacle_map); 
+
+  // TODO: Make simplification a parameter
+  
+  std::vector<std::vector<cv::Point>> vectors_outline;
+  cv::Mat obstacle_map_open;
+  if (simplify >= 1) {
+    int open_kernel_size = 3;  // 0.15m at 5cm pixel resolution
+    cv::Mat kernel = cv::getStructuringElement( cv::MORPH_ELLIPSE, {open_kernel_size*2+1, open_kernel_size*2+1});
+    cv::morphologyEx(obstacle_map, obstacle_map_open, cv::MORPH_OPEN, kernel); 
+  } else {
+    obstacle_map_open = obstacle_map.clone();
   }
 
-  // pad the left and right of the map each with an empty column (255).
-  cv::copyMakeBorder(obstacle_map, padded_map, 0, 0, 1, 1, cv::BORDER_CONSTANT,
-                     255);
-
-  // loop through all the columns, looking at 2 at once
-  for (int i = 0; i < padded_map.cols - 1; i++) {
-    cv::Mat col1 = padded_map.col(i);
-    cv::Mat col2 = padded_map.col(i + 1);
-    cv::Mat diff;
-
-    cv::absdiff(col1, col2, diff);
-
-    int start = 0;
-    bool started = false;
-
-    for (unsigned int j = 0; j <= diff.total(); j++) {
-      bool edge_exists = false;
-      if (j < diff.total()) {
-        edge_exists = diff.at<uint8_t>(j, 0);
-      }
-
-      if (edge_exists && !started) {
-        start = j;
-        started = true;
-      } else if (started && !edge_exists) {
-        add_edge(i, start, i, j);
-
-        started = false;
-      }
+  cv::findContours(obstacle_map_open, vectors_outline, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE);
+  for (auto& polygon : vectors_outline) {
+    std::vector<cv::Point2f> polygon2f;  // create a double rep. for RDP accuracy
+    std::transform(polygon.begin(), polygon.end(), std::back_inserter(polygon2f),
+               [](const cv::Point& p) { return (cv::Point2f)p; });
+    std::vector<cv::Point2f>& poly_to_use = polygon2f;
+
+    
+    if (simplify >= 2) {
+      std::vector<cv::Point2f> polygon_rdp;
+      cv::approxPolyDP(polygon2f, polygon_rdp, 1.0, true);  // RDP reduction
+      if (polygon_rdp.size()>4) poly_to_use = polygon_rdp;
     }
+
+    add_poly(poly_to_use);
   }
+
+  ROS_INFO_NAMED("Layer", "added %d line segments", edges_added);
 }
 
 void Layer::DebugVisualize() const {

From 6601c6560190372f304a45f3a0f758ea9ca8ea5b Mon Sep 17 00:00:00 2001
From: Joseph Duchesne <joseph@avidbots.com>
Date: Tue, 7 May 2024 17:05:02 -0400
Subject: [PATCH 4/6] Added simplify_map feature, as well as benchmark. New
 simplify_map can speed up performance 10-20%, and is baseline faster at =0

---
 docs/core_functions/ros_launch.rst            |   6 +-
 flatland_plugins/CMakeLists.txt               |   2 +-
 flatland_server/CMakeLists.txt                |  20 ++-
 .../include/flatland_server/layer.h           |   3 +-
 .../flatland_server/simulation_manager.h      |   5 +-
 flatland_server/launch/benchmark.launch       |  33 +++++
 flatland_server/package.xml                   |   2 +-
 flatland_server/src/flatland_benchmark.cpp    | 136 ++++++++++++++++++
 flatland_server/src/flatland_server_node.cpp  |   2 +-
 flatland_server/src/layer.cpp                 |  10 +-
 flatland_server/src/simulation_manager.cpp    |  72 +++++++---
 .../test/benchmark_world/cleaner.model.yaml   |  94 ++++++++++++
 flatland_server/test/benchmark_world/map.png  | Bin 0 -> 7537 bytes
 flatland_server/test/benchmark_world/map.yaml |   7 +
 .../test/benchmark_world/map3d.png            | Bin 0 -> 7569 bytes
 .../test/benchmark_world/map3d.yaml           |   6 +
 .../test/benchmark_world/turtlebot.model.yaml |  53 +++++++
 .../test/benchmark_world/world.yaml           |  16 +++
 .../test/conestogo_office_test/world.yaml     |  16 +--
 19 files changed, 444 insertions(+), 39 deletions(-)
 create mode 100644 flatland_server/launch/benchmark.launch
 create mode 100644 flatland_server/src/flatland_benchmark.cpp
 create mode 100644 flatland_server/test/benchmark_world/cleaner.model.yaml
 create mode 100644 flatland_server/test/benchmark_world/map.png
 create mode 100644 flatland_server/test/benchmark_world/map.yaml
 create mode 100644 flatland_server/test/benchmark_world/map3d.png
 create mode 100644 flatland_server/test/benchmark_world/map3d.yaml
 create mode 100644 flatland_server/test/benchmark_world/turtlebot.model.yaml
 create mode 100644 flatland_server/test/benchmark_world/world.yaml

diff --git a/docs/core_functions/ros_launch.rst b/docs/core_functions/ros_launch.rst
index 81c6eb0d..9dcde888 100644
--- a/docs/core_functions/ros_launch.rst
+++ b/docs/core_functions/ros_launch.rst
@@ -25,7 +25,8 @@ Here are the full list of parameters with the default values
                                             step_size:=0.005 \
                                             show_viz:=true \
                                             viz_pub_rate:=30.0 \
-                                            use_rviz:=false
+                                            use_rviz:=false \
+                                            simplify_map:=2
 
 * **world_path**: path to world.yaml
 * **update_rate**: the real time rate to run the simulation loop in Hz
@@ -33,4 +34,5 @@ Here are the full list of parameters with the default values
 * **show_viz**: show visualization, pops the flatland_viz window and publishes 
   visualization messages, either true or false
 * **viz_pub_rate**: rate to publish visualization in Hz, works only when show_viz=true
-* **use_rviz**:  works only when show_viz=true, set this to disable flatland_viz popup
\ No newline at end of file
+* **use_rviz**:  works only when show_viz=true, set this to disable flatland_viz popup
+* **simplify_map**: Simpify map during vector tracing: 0=None (default), 1=moderately, 2=significantly
\ No newline at end of file
diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt
index d0cee763..01327750 100644
--- a/flatland_plugins/CMakeLists.txt
+++ b/flatland_plugins/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2)
 project(flatland_plugins)
 
 # Ensure we're using c++11
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3")
 set(CMAKE_BUILD_TYPE RelWithDebInfo)
 
 ## Find catkin macros and libraries
diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt
index 12e7d9a5..f832c613 100644
--- a/flatland_server/CMakeLists.txt
+++ b/flatland_server/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2)
 project(flatland_server)
 
 # Ensure we're using c++11
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3")
 
 set(CMAKE_BUILD_TYPE RelWithDebInfo)
 
@@ -73,6 +73,7 @@ catkin_package(
 include_directories(
   thirdparty
   include
+  thirdparty/Clipper2Lib/CPP/Clipper2Lib/include
   ${catkin_INCLUDE_DIRS}
   ${YAML_CPP_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
@@ -139,6 +140,23 @@ target_link_libraries(flatland_server
   flatland_lib
 )
 
+## Add benchmark non-installed binary
+add_executable(flatland_benchmark src/flatland_benchmark.cpp)
+add_dependencies(flatland_benchmark ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(flatland_benchmark
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${LUA_LIBRARIES}
+  Threads::Threads
+  flatland_Box2D
+  yaml-cpp
+  flatland_lib
+)
+
+
 #############
 ## Install ##
 #############
diff --git a/flatland_server/include/flatland_server/layer.h b/flatland_server/include/flatland_server/layer.h
index 8a6727a7..137ccee6 100644
--- a/flatland_server/include/flatland_server/layer.h
+++ b/flatland_server/include/flatland_server/layer.h
@@ -151,10 +151,9 @@ class Layer : public Entity {
    * @param[in] bitmap OpenCV Image
    * @param[in] occupied_thresh Threshold indicating obstacle if above
    * @param[in] resolution Resolution of the map image in meters per pixel
-   * @param[in] simplify Simplification factor: 0=None, 1=moderate, 2=significant
    */
   void LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh,
-                      double resolution, int simplify=0);
+                      double resolution);
 
   /**
    * @brief Visualize layer for debugging purposes
diff --git a/flatland_server/include/flatland_server/simulation_manager.h b/flatland_server/include/flatland_server/simulation_manager.h
index b85bac0b..03f4a58c 100644
--- a/flatland_server/include/flatland_server/simulation_manager.h
+++ b/flatland_server/include/flatland_server/simulation_manager.h
@@ -64,6 +64,8 @@ class SimulationManager {
   bool show_viz_;                ///< flag to determine if to show visualization
   double viz_pub_rate_;          ///< rate to publish visualization
   std::string world_yaml_file_;  ///< path to the world file
+  Timekeeper timekeeper_;        ///< Timekeeper manager
+  uint64_t iterations_ = 0;      ///< Main loop iteration count (for debugging/profiling)
 
   /**
    * @name  Simulation Manager constructor
@@ -79,8 +81,9 @@ class SimulationManager {
 
   /**
    * This method contains the loop that runs the simulation
+   * @param[in] benchmark optional, default false, ignore update timer (run as fast as possible)
    */
-  void Main();
+  void Main(bool benchmark=false);
 
   /**
    * Kill the world
diff --git a/flatland_server/launch/benchmark.launch b/flatland_server/launch/benchmark.launch
new file mode 100644
index 00000000..2c5f1b5e
--- /dev/null
+++ b/flatland_server/launch/benchmark.launch
@@ -0,0 +1,33 @@
+<launch>
+
+  <!--
+  You can override these default values:
+    roslaunch flatland_Server server.launch world_path:="/some/world.yaml" initial_rate:="30.0"
+  -->
+  <arg name="world_path"   default="$(find flatland_server)/test/benchmark_world/world.yaml"/>
+  <arg name="update_rate" default="200.0"/>
+  <arg name="step_size" default="0.005"/>
+  <arg name="benchmark_duration" default="60.0"/>
+  <arg name="use_perf" default="false" />
+  <arg name="show_viz" default="true" />
+  <arg name="simplify_map" default="2" />  <!-- 0=None, 1=moderately, 2=significantly-->
+
+  <env name="ROSCONSOLE_FORMAT" value="[${severity} ${time} ${logger}]: ${message}" />
+
+  <param name="use_sim_time" value="true"/>  
+
+  <!-- launch flatland benchmark -->
+  <!-- /usr/lib/linux-tools/5.4.0-173-generic/perf on my local container -->
+  <node name="flatland_benchmark" pkg="flatland_server" type="flatland_benchmark" output="screen" required="true" 
+    launch-prefix="$(eval 'perf record --call-graph dwarf --output=perf.out.flatland_benchmark.data --' if arg('use_perf') else '')"
+    >  
+    <!-- Use the arguments passed into the launchfile for this node -->
+    <param name="world_path" value="$(arg world_path)" />
+    <param name="update_rate" value="$(arg update_rate)" />
+    <param name="step_size" value="$(arg step_size)" />
+    <param name="show_viz" value="$(arg show_viz)" />
+    <param name="benchmark_duration" value="$(arg benchmark_duration)" />
+    <param name="simplify_map" value="$(arg simplify_map)" />
+  </node>
+
+</launch>
diff --git a/flatland_server/package.xml b/flatland_server/package.xml
index 65f9aaea..f1609882 100644
--- a/flatland_server/package.xml
+++ b/flatland_server/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flatland_server</name>
-  <version>1.4.1</version>
+  <version>1.5.0</version>
   <description>The flatland_server package</description>
   <license>BSD</license>
   <url type="website">https://bitbucket.org/avidbots/flatland</url>
diff --git a/flatland_server/src/flatland_benchmark.cpp b/flatland_server/src/flatland_benchmark.cpp
new file mode 100644
index 00000000..3b74f475
--- /dev/null
+++ b/flatland_server/src/flatland_benchmark.cpp
@@ -0,0 +1,136 @@
+/*
+ *  ______                   __  __              __
+ * /\  _  \           __    /\ \/\ \            /\ \__
+ * \ \ \L\ \  __  __ /\_\   \_\ \ \ \____    ___\ \ ,_\   ____
+ *  \ \  __ \/\ \/\ \\/\ \  /'_` \ \ '__`\  / __`\ \ \/  /',__\
+ *   \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\
+ *    \ \_\ \_\ \___/  \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/
+ *     \/_/\/_/\/__/    \/_/\/__,_ /\/___/  \/___/  \/__/\/___/
+ * @copyright Copyright 2024 Avidbots Corp.
+ * @name	flatland_benchmark.cpp
+ * @brief	Benchmark version of flatland_server
+ * @author Joseph Duchesne
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, Avidbots Corp.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *      copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Avidbots Corp. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <ros/ros.h>
+#include <signal.h>
+#include <string>
+
+#include "flatland_server/simulation_manager.h"
+
+/** Global variables */
+flatland_server::SimulationManager *simulation_manager;
+double benchmark_duration = 10.0; // overwritten by rosparam "benchmark_duration" if set
+double benchmark_start = 0.0f;
+
+/**
+ * @name        SigintHandler
+ * @brief       Interrupt handler - sends shutdown signal to simulation_manager
+ * @param[in]   sig: signal itself
+ */
+void SigintHandler(int sig) {
+  ROS_WARN_NAMED("Benchmark", "*** Shutting down... ***");
+
+  if (simulation_manager != nullptr) {
+    simulation_manager->Shutdown();
+    delete simulation_manager;
+    simulation_manager = nullptr;
+  }
+  ROS_INFO_STREAM_NAMED("Benchmark", "Beginning ros shutdown");
+  ros::shutdown();
+}
+
+void CheckTimeout(const ros::WallTimerEvent& timer_event) {
+  double elapsed = simulation_manager->timekeeper_.GetSimTime().toSec();
+  if (elapsed >= benchmark_duration) {
+    double bench_time = ros::WallTime::now().toSec() - benchmark_start;
+    ROS_INFO_NAMED("Benchmark", "Benchmark complete. Ran %ld iterations in %.3f sec; %.3f iter/sec. Shutting down.",
+      simulation_manager->iterations_, bench_time, (double)(simulation_manager->iterations_)/bench_time);
+    ros::shutdown();
+  }
+  ROS_INFO_THROTTLE_NAMED(1.0, "Benchmark", "Elapsed: %.1f/%.1f.", elapsed, benchmark_duration);
+}
+
+/**
+ * @name        main
+ * @brief       Entrypoint for Flatland Server ros node
+ */
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "flatland", ros::init_options::NoSigintHandler);
+  ros::NodeHandle node_handle("~");
+
+  // todo: Load default parameters, run on a specific (default incl.) map for N seconds, then report.
+
+  // Load parameters
+  std::string world_path;  // The file path to the world.yaml file
+  if (!node_handle.getParam("world_path", world_path)) {
+    ROS_FATAL_NAMED("Benchmark", "No world_path parameter given!");
+    ros::shutdown();
+    return 1;
+  }
+
+  float update_rate = 200.0;  // The physics update rate (Hz)
+  node_handle.getParam("update_rate", update_rate);
+
+  float step_size = 1 / 200.0;
+  node_handle.getParam("step_size", step_size);
+
+  bool show_viz = false;
+  node_handle.getParam("show_viz", show_viz);
+
+  float viz_pub_rate = 30.0;
+  node_handle.getParam("viz_pub_rate", viz_pub_rate);
+
+  node_handle.getParam("benchmark_duration", benchmark_duration);
+
+  // Create simulation manager object
+  simulation_manager = new flatland_server::SimulationManager(
+      world_path, update_rate, step_size, show_viz, viz_pub_rate);
+
+  // Register sigint shutdown handler
+  signal(SIGINT, SigintHandler);
+
+  // timeout
+  ros::WallTimer timeout_timer = node_handle.createWallTimer(ros::WallDuration(0.01), &CheckTimeout);
+
+  ROS_INFO_STREAM_NAMED("Benchmark", "Initialized");
+  benchmark_start = ros::WallTime::now().toSec();
+  simulation_manager->Main(/*benchmark=*/true);
+
+  ROS_INFO_STREAM_NAMED("Benchmark", "Returned from simulation manager main");
+  delete simulation_manager;
+  simulation_manager = nullptr;
+  return 0;
+}
diff --git a/flatland_server/src/flatland_server_node.cpp b/flatland_server/src/flatland_server_node.cpp
index 871b666b..1de4d4a3 100644
--- a/flatland_server/src/flatland_server_node.cpp
+++ b/flatland_server/src/flatland_server_node.cpp
@@ -7,7 +7,7 @@
  *    \ \_\ \_\ \___/  \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/
  *     \/_/\/_/\/__/    \/_/\/__,_ /\/___/  \/___/  \/__/\/___/
  * @copyright Copyright 2017 Avidbots Corp.
- * @name	flatland_server_ndoe.cpp
+ * @name	flatland_server_node.cpp
  * @brief	Load params and run the ros node for flatland_server
  * @author Joseph Duchesne
  *
diff --git a/flatland_server/src/layer.cpp b/flatland_server/src/layer.cpp
index 3c1f515e..8c182f58 100644
--- a/flatland_server/src/layer.cpp
+++ b/flatland_server/src/layer.cpp
@@ -218,7 +218,7 @@ void Layer::ReadLineSegmentsFile(const std::string &file_path,
 }
 
 void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh,
-                           double resolution, int simplify) {
+                           double resolution) {
   uint16_t category_bits = cfr_->GetCategoryBits(names_);
 
   uint32_t edges_added = 0;
@@ -251,11 +251,13 @@ void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh,
   // considered to be occupied
   cv::inRange(bitmap, occupied_thresh, 1.0, obstacle_map); 
 
-  // TODO: Make simplification a parameter
+  // simplify_map rosparam: 0=None, 1=moderate, 2=maximum simplification of map polygon outlines
+  int simplify = 0;
+  ros::param::param<int>("simplify_map", simplify, 0);
   
   std::vector<std::vector<cv::Point>> vectors_outline;
   cv::Mat obstacle_map_open;
-  if (simplify >= 1) {
+  if (simplify >= 2) {
     int open_kernel_size = 3;  // 0.15m at 5cm pixel resolution
     cv::Mat kernel = cv::getStructuringElement( cv::MORPH_ELLIPSE, {open_kernel_size*2+1, open_kernel_size*2+1});
     cv::morphologyEx(obstacle_map, obstacle_map_open, cv::MORPH_OPEN, kernel); 
@@ -271,7 +273,7 @@ void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh,
     std::vector<cv::Point2f>& poly_to_use = polygon2f;
 
     
-    if (simplify >= 2) {
+    if (simplify >= 1) {
       std::vector<cv::Point2f> polygon_rdp;
       cv::approxPolyDP(polygon2f, polygon_rdp, 1.0, true);  // RDP reduction
       if (polygon_rdp.size()>4) poly_to_use = polygon_rdp;
diff --git a/flatland_server/src/simulation_manager.cpp b/flatland_server/src/simulation_manager.cpp
index 862e4985..684b71d7 100644
--- a/flatland_server/src/simulation_manager.cpp
+++ b/flatland_server/src/simulation_manager.cpp
@@ -54,6 +54,8 @@
 #include <exception>
 #include <limits>
 #include <string>
+#include <chrono>
+#include <thread>
 
 namespace flatland_server {
 
@@ -73,7 +75,7 @@ SimulationManager::SimulationManager(std::string world_yaml_file,
                  show_viz_ ? "true" : "false", viz_pub_rate_);
 }
 
-void SimulationManager::Main() {
+void SimulationManager::Main(bool benchmark) {
   ROS_INFO_NAMED("SimMan", "Initializing...");
   run_simulator_ = true;
 
@@ -87,16 +89,23 @@ void SimulationManager::Main() {
 
   if (show_viz_) world_->DebugVisualize();
 
-  int iterations = 0;
+  iterations_ = 0;
   double filtered_cycle_util = 0;
   double min_cycle_util = std::numeric_limits<double>::infinity();
   double max_cycle_util = 0;
   double viz_update_period = 1.0f / viz_pub_rate_;
   ServiceManager service_manager(this, world_);
-  Timekeeper timekeeper;
 
-  ros::WallRate rate(update_rate_);
-  timekeeper.SetMaxStepSize(step_size_);
+  // ros::WallDuration(1.0).sleep(); // sleep for one second to allow world/plugins to init
+
+  // integrated ros::WallRate logic here to expose internals for benchmarking
+  std::chrono::duration<double> start = std::chrono::steady_clock::now().time_since_epoch();
+  std::chrono::duration<double> expected_cycle_time(1.0/update_rate_);
+  std::chrono::duration<double> actual_cycle_time(0.0);
+  using seconds_d = std::chrono::duration<double, std::ratio<1, 1>>;
+  double seconds_taken = 0;
+
+  timekeeper_.SetMaxStepSize(step_size_);
   ROS_INFO_NAMED("SimMan", "Simulation loop started");
 
   while (ros::ok() && run_simulator_) {
@@ -104,33 +113,60 @@ void SimulationManager::Main() {
     // see flatland_plugins/update_timer.cpp for this formula
     double f = 0.0;
     try {
-      f = fmod(ros::WallTime::now().toSec() +
-                   (rate.expectedCycleTime().toSec() / 2.0),
+      f = fmod(ros::Time::now().toSec() +
+                   (expected_cycle_time.count() / 2.0),
                viz_update_period);
     } catch (std::runtime_error& ex) {
       ROS_ERROR("Flatland runtime error: [%s]", ex.what());
     }
-    bool update_viz = ((f >= 0.0) && (f < rate.expectedCycleTime().toSec()));
+    std::chrono::duration<double> update_start = std::chrono::steady_clock::now().time_since_epoch();
+    bool update_viz = ((f >= 0.0) && (f < expected_cycle_time.count()));
 
-    world_->Update(timekeeper);  // Step physics by ros cycle time
+    world_->Update(timekeeper_);  // Step physics by ros cycle time
 
     if (show_viz_ && update_viz) {
       world_->DebugVisualize(false);  // no need to update layer
       DebugVisualization::Get().Publish(
-          timekeeper);  // publish debug visualization
+          timekeeper_);  // publish debug visualization
     }
 
     ros::spinOnce();
-    rate.sleep();
 
-    iterations++;
+    seconds_taken += (seconds_d(std::chrono::steady_clock::now().time_since_epoch()) - update_start).count();
+
+    // ros::WallRate::sleep() logic, but using std::chrono time
+    {
+      std::chrono::duration<double> expected_end = start + expected_cycle_time;
+      std::chrono::duration<double> actual_end = std::chrono::steady_clock::now().time_since_epoch();
+      std::chrono::duration<double> sleep_time = expected_end - actual_end;  //calculate the time we'll sleep for
+      actual_cycle_time = actual_end - start;
+      start = expected_end;  //make sure to reset our start time
+      // ROS_INFO_NAMED(
+      //   "SimMan", "actual_end: %f, start: %f, actual: %f", 
+      //   seconds_d(actual_end).count(), 
+      //   seconds_d(start).count(), 
+      //   seconds_d(actual_cycle_time).count());
+      if(sleep_time.count() <= 0.0) { //if we've taken too much time we won't sleep
+        if (actual_end > expected_end + expected_cycle_time) {
+          start = actual_end;
+        }
+      } else {  // sleep, unless we're in a benchmark
+        if (benchmark == false) {   // if benchmark==true, skip sleeping to run as fast as possible
+          std::this_thread::sleep_for(sleep_time);
+        } else {
+          start = actual_end;
+        }
+      }
+    }
+
+    iterations_++;
 
-    double cycle_time = rate.cycleTime().toSec() * 1000;
-    double expected_cycle_time = rate.expectedCycleTime().toSec() * 1000;
-    double cycle_util = cycle_time / expected_cycle_time * 100;  // in percent
-    double factor = timekeeper.GetStepSize() * 1000 / expected_cycle_time;
+    double cycle_time = actual_cycle_time.count() * 1000;
+    double expected_cycle_time_ms = expected_cycle_time.count() * 1000;
+    double cycle_util = cycle_time / expected_cycle_time_ms * 100;  // in percent
+    double factor = timekeeper_.GetStepSize() * 1000 / expected_cycle_time_ms;
     min_cycle_util = std::min(cycle_util, min_cycle_util);
-    if (iterations > 10) max_cycle_util = std::max(cycle_util, max_cycle_util);
+    if (iterations_ > 10) max_cycle_util = std::max(cycle_util, max_cycle_util);
     filtered_cycle_util = 0.99 * filtered_cycle_util + 0.01 * cycle_util;
 
     ROS_INFO_THROTTLE_NAMED(
@@ -138,7 +174,7 @@ void SimulationManager::Main() {
         "utilization: min %.1f%% max %.1f%% ave %.1f%%  factor: %.1f",
         min_cycle_util, max_cycle_util, filtered_cycle_util, factor);
   }
-  ROS_INFO_NAMED("SimMan", "Simulation loop ended");
+  // std::cout << "Simulation loop ended. " << iterations_ << " iterations in " << seconds_taken << " seconds, " <<  (double)iterations_/seconds_taken << " iterations/sec" << std::endl;
 
   delete world_;
 }
diff --git a/flatland_server/test/benchmark_world/cleaner.model.yaml b/flatland_server/test/benchmark_world/cleaner.model.yaml
new file mode 100644
index 00000000..a6d17f06
--- /dev/null
+++ b/flatland_server/test/benchmark_world/cleaner.model.yaml
@@ -0,0 +1,94 @@
+
+bodies:
+  - name: base
+    type: dynamic  
+    color: [1, 1, 1, 0.75] 
+    footprints:
+      - type: polygon
+        density: 100
+        points: [ [-1.03, -0.337],
+                  [.07983, -0.337],
+                  [.30, -.16111],
+                  [.30, .16111],
+                  [.07983, 0.337],
+                  [-1.03, 0.337] ]
+  - name: front_wheel
+    color: [1, 1, 1, 0.75] 
+    footprints:
+      - type: polygon
+        density: 1.0
+        points: [[ 0.0875, -0.0250],
+                 [ 0.0875,  0.0250],
+                 [-0.0875, 0.0250],
+                 [-0.0875, -0.0250]]
+
+  - name: rear_left_wheel
+    color: [1, 1, 1, 0.75] 
+    footprints:
+      - type: polygon
+        density: 1.0
+        points: [[ 0.0875, -0.0255],
+                 [ 0.0875,  0.0255],
+                 [-0.0875, 0.0255],
+                 [-0.0875, -0.0255]]
+
+  - name: rear_right_wheel
+    color: [1, 1, 1, 0.75]
+    footprints:
+      - type: polygon
+        density: 1.0
+        points: [[ 0.0875, -0.0255],
+                 [ 0.0875,  0.0255],
+                 [-0.0875, 0.0255],
+                 [-0.0875, -0.0255]]
+
+joints:
+  - type: revolute
+    name: front_wheel_revolute
+    bodies: 
+      - name: front_wheel
+        anchor: [0, 0]
+      - name: base
+        anchor: [0, 0]
+
+  - type: weld
+    name: rear_right_wheel_weld
+    bodies: 
+      - name: rear_left_wheel
+        anchor: [0, 0]
+      - name: base
+        anchor: [-0.83, 0.29]
+
+  - type: weld
+    name: rear_left_wheel_weld  
+    bodies: 
+      - name: rear_right_wheel
+        anchor: [0, 0]
+      - name: base
+        anchor: [-0.83, -0.29]
+
+plugins:
+  - type: ModelTfPublisher
+    name: tf_publisher
+    publish_tf_world: true
+
+  - type: TricycleDrive
+    name: cleaner_drive 
+    body: base
+    front_wheel_joint: front_wheel_revolute
+    rear_left_wheel_joint: rear_left_wheel_weld
+    rear_right_wheel_joint: rear_right_wheel_weld
+    odom_frame_id: map
+
+  - type: Laser
+    name: laser_front
+    frame: laser_front
+    topic: scan
+    body: base
+    broadcast_tf: true
+    origin: [0.28, 0, 0]
+    range: 20
+    always_publish: true
+    angle: {min: -2.356194490192345, max: 2.356194490192345, increment: 0.004363323129985824}
+    noise_std_dev: 0.05
+    update_rate: 40
diff --git a/flatland_server/test/benchmark_world/map.png b/flatland_server/test/benchmark_world/map.png
new file mode 100644
index 0000000000000000000000000000000000000000..a29b0ba1956bf88be85462a5c4eab4690a02fccf
GIT binary patch
literal 7537
zcmai32UL^WvJRkvg{BBd2}+J2N)c%W0;qtYqX|vAAfg~O^b+uhNRdPY=@6w#F^KdU
ziV&oVl+XhNh?D@KhZ0`SJ@>wQ*Inzax7Ob4|M%=YvuD14*6f-6M?ElvvY!z+0{{Tn
zb+k2M0KiEg0Ki0JV-Nr>$|2tv4`zEks3rhV7z?_^7@hXeeuMx3&R+PdGXYZ5`58o3
zZyf_I)@dfr3%ty5$Gf}$fH1R;=3Qg|G0LQCYGG~Vsh`G~lULmKiQz(l^2>C9F%Q>8
z7m2r?y;u_wq`iO2OD?1{^R#It8{j)A%>M)z8sNT;(YlilLF8NX9$V9~2Mb%t6*(5p
zy7OBVgBg+;NkshDKd^}%dw91}gUN!NF{z)()4L-07G|5W-v{I8IN{D!Y|gYLF}2MN
zmz2FA1BqM_C^kS%7~_JeICrd<{{cMot7M^7so)%wrQY#z<7na)zh&_AgNy!qkM~yH
zuCYgD+%FBGG8^^b0@M;yVNsQu&Rt*a@`h)8QoMtz=-^HYW_dmzX|qrl*<=}KHZi+(
zJkV3*KW8-HiQ6y6e5SRlCpzmKn+O<wpB*X@onN-iL_Saz6(+t*Ak7Cx>&NUJsA*Fp
zeRku<o@~KwI6B^ENs&8${W_=&=0VrX06|8+c01g$o7Ch82M{y$b>0PVqWgO#m+Ug?
z?+yhem*WOs+O$TycX@C;<mlAK?5X?}hwPid@4@uF(+;jdlY!ZO^e1f~r7wR-#`+Z5
zhZZBeV^+!M!CFWO;W{Yla0MSnwhcrs7>JabL_#a2FLB1BB-$t#QRwRVYdG_*?=A&A
zC4<&bclIOGzDIJ`E-BfRwb$^*!8FgQhYzo_x818c@f}oOvC#p}oj86XxTVl`Z@4Bx
zi3gZnQM~+uO#gX5@f~sNN+EE@b#a6b#VwJq{pdE*gsC7fT<N}Jj<y5_ZSTaY+UTs3
z9~i!y^d5X~e$O#C+dq7l>|mxDku7IiWj(+UI4Ep(V~oZF&RBM*T`pVVp28%NKdFAB
zrm4wO+14nz^HGL^#QvHc&*pJ6xzjaiXwI*J!VL(@<}0NVM_<JyE0%a#GWpDf#*cyW
zUs>rN5yA!PA?^2Q?>T#$fc`<`+O!L|$O)j=-iL(-X-nL6QIYLJT<t#V9SCzI6hLMB
z%GBDzl-PNn%Jvb#{#Oe;;SZILhKBu^s_;Kit&5igGOg~EGVznM{7*bk{hR4Bkm)<9
zNqsim1u*n4kM3+^__b12%-@p!UNJ0`%9Q*?s0|d>tMIqu&vO53<IBMQ^G&+V9N}7k
z|0FU5|7)cbQ!&p!=Dh5BYfe$w{^oeutMHfNk%<7)GXQD_>$f%Tf3iF~ZI?SOyb^Tc
z@5!)y75FT7hD&m>U>j(k+b$QtP|&Dsjoa>%8xI;#2wkmYy|dl%BnV)P1~{JrWVrmZ
zQ9E!H1Hg|)=-<Zqr+^56vp(7XF#j`<iLrXE=n4}<*#DvT-xmJcj(?29IH?#dz)cgy
zLLifv%Frygovav>wgkIb4KErXN4v(LZa#!C>!1IB#8NwEUjGv@i11s62HOl={sXjs
z!}~Y!zsLWr_J5E6OYQ%SSpT=J|FL~2Nj)Zvg_qg0L^twB9aL9h{;PZ&Na(hK!*z9^
z%I}~RlN_O{D@kwcW$}ZUH$lCc;;f5aP1sghFA;>9HRU(q_3sTtUS>k{EVu83WAEnj
zT?{e~WW5&DEs@&+*3r&z8F7)lX~v<6dX3r!Uy;%n0<DxGGGLtn63Pzc@zfo~Lg)}k
zNE(bd*XG#m-Rz>P1-=4&j0wq2h3PtCzZJV0T=ijnme(SiYayx0kd{3b@<lFGTCZW_
zTd*xdKd`DEOjr~Mu1>}Rb3wCl8flz&L!AD?B-})G8H<(N^y{bcqCKYX;|6Pn*M?zX
zKQ>BBxfq;QDQh--MQ79y9Qv#RR5s3}$Qbe7%fQEL&HQ>w9ev-g&vNgKeNiLYORH$~
zSwdQ-^g7IoWyq#cD?cmz!$U0VFSp!#{fx{A5FvGzEbXx8i_?|<;ji)>9$8#%u>b)d
z;}-q%n+bKJOM8s<*3JWyOANn%Qw+3KYqBRY*o13s1x?$o&c=ZX{WvX&KX5m99zt5w
zity~Ib<8jbGduzP*Eh5{VfVT@pWP2!#n2Mo%x`-7)bXPeH>LSEZr?=Ab;Nz^ghBoc
z6ADd(tA)JjRS;;AW#_BK7UUVb96R`2pOHP<o6zyg`$;bfMk%S&8>T#04X;c3JZ1p8
zL;TSzvd)3n&+h`~CoGA0V`BhM9pm5P9xd`l;|G$u?m#ft;m1!J4#T8|Y=e0*>8OTd
zUWYF^NvT=|SFO8CgZDm`=T?^~*9T=3J8CH>M!ol(9aoC*qlYiatDLLwf0m~oV(T>i
z<9nfP+6<ysBKI2?r(<Q@u#e2mkS%?<2BvsxJSxWTk&L^?v6Pi`3rvsFm;=)*%uP&i
z-i?uAU#udNqWi9I-~=Wej@R;O1$@?#o;PY<6a@@6?OO>b&0C4Xt{9^G^A`3M`KY`D
z2A~aCyxUU&vxYB^T`-#DWSOK7--DP*bz(Am;k|<@wr%$CCG{d?{6#+&$*G!iblRIZ
z^?9xDxRCv>5VYg-*HQg{#yFYPK3h|3t$uGl7OjLZ`cSeD&kaT7$25_j={e61Mm>L{
z`yzWOr+Bf$v+n9){PriwqXKDMguhFypBn}9vn*Ca{V1c?C&$;>>-{r$>Y<(>CGqTK
zXedYP5p9i6q&g!NsJyR}Z8S5IGm`gb{k4Z(Jr);_kh}D{)im@-+ve$pT6LXv<rOa5
z?%q8@*X6b2>^+WS_E^!p+V%?))s=~9he1Jb;M8%rjXpjKhenS2%6yspY@N}$oW}Cb
z^Wds{8_oo365@BOoAOkYP7~A1-s7qnw`*S(fvFINg0tewJ}4}X$V*gEp2lnrzT$K}
z)TpDr;<;b|nWax_ql{G*1NP-G#K=m&5?+X$W)@aj#8%qO$@VD%xxZ8Bbr|9+OQ%0y
zDx)bPv|gzUrSs0j72=b@_qQI<1`Hh^SQkS{AJo1Va@8h%tdXbqe#o}zH#B9d{tU#J
z7r8GrE-aj)G^kYYWg|@u9^&p=D+Xq1ODIq(yWncb0QvhaV2?}aQmr!+=SC$Id=Pi7
z53$x&pLz#ay=~E%Y5bMaH&>O&K_GtlOCeD~7p;PD<uP7IdjYO#F$P&4v!x}!N{0r~
zpH1jxia?RNEemVnY?g$dg-vllNC|H=+5Nr89P(9BG}NXDSh7Ya9upvM8_}0WEiWvN
zut7~-kA*WIM}B%^FKWtzB&W(^PaU^eu_gXOjXyzF1PXn-sT6-&T8OitxK8I9FhBzN
zbd4a7_Gvy4%5aIN&K8}GO@H{@pG;CB3uRv(T?(#MX;?|?*_uyzOWAHt@qgNdyJh?V
zRfuofDGgC*-Ze#2E{Fu`R~0N|xT>hHXD$eqc|fpcoO(_b^QGRcEt|`v7D6~_FTKT_
z8fo3MUB>o3`$jVe?%Z^gm`f8X2@K8|x8SR6_0z-mXE@t5I1*RnpMC(9y!d1L3z`o-
zpW^!gqm56=v0j?pg%?5mnfE4c@Sq?WnWOV&s7I>U)ymA660Z<+<)e6%W8Otyxy!w7
zX_!<Y$pqAki7Hc7pe&G(N4n=a+-(|h)=!y`<(Qq>b@=eo3|sxgN$a8c$RNkrx#tlC
zVcKS_ZVdsCRw(gV{oGyXArH?URtqI~Oz$VNKeeXrnQwo{KNyqEKwFddr<Nm~(vCYl
z%AKC?HjtdIzeJ4xy2HVbRv_b?BjcQO%gxJzsH<JwCF!7S$|m+#d_(90Wy$TCK~=V|
z*sae&Z=qS>%-ZJt4Aj{1V@MfDS)Pl!=heDwVUyt!Mqb^<Sjlb1fo?59U#Qls6B9$o
z{iZb;LZ>g$XmXrD5#Qm)?fy}BpHcnFccu>9Xd_mGpaTRV4V_=apSV9FM(Y=&nRB?T
zPe=$0GtT)wcTXo|fRFZVuh1o*xs74tTR-EL-xbBo&ty>Yy&(&xp7A$R4YE@g&;#Gc
ze^H}asT<yFvmYw$81daI7s7?AqWJ@b=#3urK|i==Iu2Ss2)8*{%Vw?Ly5xT90oix^
zXyE4A`ys0p&YchuH!`pBh91ReQ~AMmwP<1L6=88hx+UbTS?4p0b$E|?rpt7f@M4AP
zQakMx>zeowK@8mAuNHA^dfx=qe>%)8RA_#AKBr2K+%dP2r*L_=oVeJqarMc<>Wz*a
zLqT-9u9;MMnS-=3nl%O=sKhTy9yI!$u$DL3&}FHff%J4QYg{Nul~=t7>NfKcy6PqX
zH<W-H79^gRAe`@-7k?9luV}SG;8@5@?drJQ)xDrJC!=%4u}SV8Q#y8U+pnmM_>3UQ
zQI#1WLV~Uo7$!G*C+6%Wh7Y;aB$;|<jya_{uSY=xj*MTovmz%@K5FdN>A%J161+MG
zpg@^5d)995BK*PV1!C1LNMh`ll?lK3WLRzT&Fj3%y#bBluB_cN0tWlvVle7M_tvad
znoCzE&Cr7KqAHis7OKdHETQ%9T)k@^>2@7xefstgg7hESyCZEQzp$`_%-?L3$tpEx
zLZv67=d&*beJ8!~Lz@y}Wd}w|o_2O@P_-MD-w2xhwv55c_)QfjjY+;BA#^{Tn&zzh
z8SI%J-0$Kl;)2s73@>3_vrAd<1whPHx6|X(!LHkTy?o=t&iCb}*@j53tatKc;cAzG
z&>ti0eD3hn<Zwm?OMKjV!E!7C7N6=JP!CmRw#1XxY#T-ln2S|UI>f`Ut<~!*^=#9Y
zMR-C>sURtt6r8Cqr%Y@9>M?y?fn$BuOZ}2%W5B@vKohVKpMSPDra-bXz4ZL;``gcq
z&o;eOCIDtVU!kfUnf^+$Zy#48bSVMG)uTOOaTt2C{^<D31VkmnL0!oRyD0z2->=ap
z02y|$mLyX==b>ix<@~n%*Zf55hEnY~*qmEA$>Yba8sC9Xq!rw`E^I}H=xW5`QQ7K=
zmZh&kD6ZX;t0O7`VKi9LvN<rL_v+0{=!n;6weP$+KON_P81b<ijPukPF$VBOXvo>#
z{3?!y1#cz^ipjI&&6Xz0@bg;hrMF*O_#y{WWR9oq3A2hVEe1ZC#PxbJO-(o)<t-JU
zW`BvZ)q{cRp+pD6ZQsNG2)=yO+&x0m6Y_r7rm9WoWew8HwYT`yclzG(2HtXp<+*8j
zp5BABnvdR{3hNbA^9X0z-B&!D1%cfXethGF1t|gomozdLR>x=Fp-Lf54gQB4NXI#~
zYkIa$iJnr)eF3Ql$8C#9!lZXtjQ`=E*RQW0%0ktB9#vhSP6-B%uY1|qQmbFRs+85Y
zRuu79m&hF$tWYQ6j-1an8AE_S`dDx$QC%ELVYy4CBdqyJ?2LND)C?{3vv1HS+KEwq
zeFhS(f+ec&K)f!!g)tI9GbDZ}`|}B^_W39eevTf%KWe)5EDcIfy=o5gW#CEBWuOVV
z$+nGDs7XtFDUDmwwR@Oe^5kUGi|uDEykU41!a<fedn8`x`V+g?NTNf<AAr2shic5f
zt<+~zjuMpDzQ)w{Hvz-imP)JN-CzKxoKJ@lMz5AIN$;vgJUMZ@i?oK&y&}paJuB;F
zS)`!=AW%n!6W#}(sE)=f@3T&?V<TQCDFM?T0Ux_cDy)CXSXL8e;x|DvvNF+J?a{d9
z){7Ooe&&V1v;-LH<`%b|u__0-n++P~xy{C?1kDA}9FJHSF#NkPnI^Vprvw7ptva?(
zs!M=vFL`5A4DtEcaQ2nFK;tV+&{(_|R?I+`YRvsyh54Wan8dO{{;aUjV=pTswlXmk
zw`?Wcnz^uF>G9KF$XHGd+p8A?Kn>Qo4UmPp8vQg!+|;IJil7V}N|SfF?FMVq+U!f=
z?9*9+D`+!lIH;U;Yjcw{Ct|*8#lk3l5)X&os_wfrCAB-a!gVDS3=>(Be_JK>Exx#+
z^wKCApDNlW{znc=K~ugr_8`+||LrS<7Vf=|{*tX~_|R`_R5N8}OxCg{dD;8L5DGxB
zUQ8cre8VWkLw@CH8|?3bf_TtP&^8B?+rXZ+7NaIGunP-)bzI!d7WNW>W#TQu-$He(
zCcK}lIBA{2>n{N;aXL+GjUx0XwHu1E+N@ZSC-M>yx>$=d9+^@QnC1^~ZYVN$naJ+`
zL2<;$ZdX#M7BMNVGwxHq3sbpoNDWjg%$fN)<bYc%rA0#aCVwmXo(Ghep(_?S?Bqor
zN*zt9dbU5)nnur3Te0pTG9n^|P|q`vOK41@98S;4rb~I6^KA*@Nay6-n$Kz4!NI{M
zqaS!FFRa+fe+}5)OS0#Ahg+<e%o64H7nARFYOF=%?s@ej!=o)gqFEf-L?%;Xw21PF
zr|HpPSSNMv`Cx^sS9xxx$WmKV1t*U$C4Xm#lt!XXJ;>O8xH%VoDS&@FQ!%Jjy`(-C
zy{y46dM4W^3%S*Ze{NLLUa_6Rf*xX@5)T_;1kW_@A57HW^MGJCpu`5_@?*W<jdka+
ze*fLE#|oya6Hti9U&JV`dq7s__fLoJOC!sH4^&rYLr~Ji4}?xIqb{NY7LE8CmY3pL
z8Si=uj|F?Qw)eWfc_oXW_|V(Lg}GC5VI|tVd9Jn)<qPe1o0*F6^#=;-Rr3@1$U=m0
z#Ftj;Y-tXtnC5$%*~l1Odf@$*FDVYGAao13j3kap40&y?Wa2wY+HS1RXDs)9c|k_n
zm`G>BrIxK&IQM>^t#KPxH3Vzb)+gSJGP7Y^*)-%UrRoJVo<E5aKts8r@7#eH<~mrG
zKwmrMHv=~5<K*$qkY@ig_;h%~*qe9pr$t}3wmDR(R2URo%JQ(^b$)W>I{hqTxmve1
znZJ(52=$r(Vx{i&xVt2$=Vz2iVQlO-xKTqjy`}lWo<}Orr#40&<qM&B&~-&oZtqK9
z$W_bbr#m~RB_OR0m{1Q@SLqY)(!7JQ=BRYoz$8t4=y>2yyQ&Sa$_93-KRQiyHv%Ne
zK42^_z_p?V)cbCeCm)kSL~&XZ#v1GA^no1EMJticQ%Tpm7Yf5b#^|7iZeyF3t2p`|
z&Vp;DG4O3jyF+&ZDh*PJRS?Pw)Q2hy<^X4Axts0}k?4Lou7^7<-s&E2mr7UZqPyAN
z6CZtfP#y`rK0a3xLoMDiW5#NvC1CZN>C41%t0|<KY?%^s8lHr)b-s*gebG9ZxxqbB
zk2oS6wQW9$xo^fIVy#NAf#2@DKTRsamxMa&oL#vB+!V}Iqwn=xnq*NXtSwo)2iZ?;
z$E75nD<9cXU0Enz79pcs^S_f;eC%wl&5NO|Y$+3O>#a5K`{oF7TttVOY06X5Fm|)m
zMpIk+!@syWxX{Oi9EI~4XdwJFFC`irv}%@YSR0XXkYBVnOc0G!XtTDCNAdY(=ryM^
zuG1Lj9Ie_pGs=8iLQU1;xA`mo!vpa?LBB5!3(v9cw?an8QesBiv-?;h@qA^neisTF
zjsn{I`sFucL>r$jGP&>eo!Xgu+)X;T@ii4TZsoJMh5|5uC#e@%Aok<IVs+w|=1JaM
zy3lEj+Bcy*&>p7-Av-d4ZzCC}Es>j-pza@ItyZb#GEJdY_xS(b+m!IfRRT`YA4A&V
zyMg;Uh*d-7AKNdT{P%TaI`Of+-alGNx4qee(D;Q<Lx=OJ1%h6}=!mX?N5+OU{*qHq
zfbFM!(AmM^4S3QkCo8<(<ZSwz3yGl41LZZi))VA*^jol&5_89E;m5nRFID6ET#oC`
zb_`sjq&pX0tz4}!f>_O^rn=pDums!q5<~>&N{htou+*XPLTJjXU`Hh8_eO4_5}zGk
z2618ySxjjeaux~H+umk`04>NcD`>XOI5J_37M_xJ_b@7@4V0mWcOyF>>wW73UwZ;Y
zjrA1U6jva?G`|ngWvK&qmxpM3cFOJRBS)kvz2?TjvPS9KF-Is5Hk#YUoVI1okiqZ6
zHdI|q($v%_M|ba%KcX%C8TMeL2}$xSAl6owNRwq!<m&YBw{Sf6fzf3Ktai)?g}}94
zJ0ce!#d7NDg)*9{lkyh#g|Zc8dv<&jUO;B@h!*?;c-<VsIsQx<Bk88GZnFqUeYjih
zaL2G^l~fw=2~fNi_;CFBn4*F~8UM$4%SZ%TWh#}}ME1pspK>Z$mLsIv+;*tz?yFC(
zlDa<5YBLm_{~|&ecJ|K(Kdd;+vnLg^d|}?f5bUQSnHHsQ(NTKXlEc%paklmtS)#}n
zo5vEQ1l&!A8|20`n5#}uslpx#6$OdL_B|C@N{i2x{QS5kdg`Jy#<gSwcZfB;eAe^n
zFnNQqt#6;R&q%^_6^4*r(KSt-4L#W#JJy43Z-CNK1M{TyKU2->4w}r2(Xs-E=_FzC
z&U36apbZZj8w1BHBJRD00hXo1<$5}NNyNFCb7?}9#@-s=z(2afhruV7SOz}Kdjs{W
z^Wc*P;=E*OO04f7si8Kg-i2Ep0ct97OGXHsx_94MDCH%KF}mUg|2UKrteMui|6G!h
zJiA7CNvRIYu62%8ZGEXrRm@FR*{ltka<j;eAxy8I6Z#mqLfEH{G?}nQr)_WkPWMAB
z%y<|gkl2B;ibZru+EC?@j+~r$@7x4MK9+}jM&9MN>Gti1ZIcEK>7uy{FZWAiMBs+~
zeP>KX=lOfOZ-=`bB+WZGA86<YES;Y9)=K{rIq@teL1^sd+GpUV$q%0V4@!N0x@M{x
zjaW{2FU?E-!S^v2q52z!V+me{V63#(0B$3&CM-3n3(s$om!y_;6+l`lO-z7CS8mP2
zq@Gi+M+8*AA5LxD_J~LRxgO*2%hfL#qRs_Q<9k<=moUZy_FKC~%)C3YCLjCdyx9FV
z&y>c{!|lh^fpV=l(EKV5;VtrDj;H3hl`>wlTh}216UX?9`mK@atRMQ03%5AUmFREd
z3oK2Re~&LdCRUuYl@67i2%;pEUe#Ka89y(9$;28GeG7#9GJp-&g^f6o$htVg_&HxC
ziMTPbTp`MQ|7Q%*Nw<WS3wNfKrP9O*b(+0LzgFlf(Ajqxefr}_-PD<vG9rI8=6S@9
z<|<j}A8{VNj<O%wNGJN?%%PS|kO^wNkAT$fjAH(<TFr>*Yx?GR;$sy8?mMk0mfq|n
zK)~FylJ1lmT=us@eGA-l4e-1leM?j8kzwo=<)y{(m{7eC$E9@T?VUWghx<}@b}Nog
zAVa0oTHEoioj$@fS&_7Wg(&2Cqxo#r^D3qTAnM+tZlPaMx97_`p4Hs0=0NcpB=W%3
zB!ny&6smVqVH|Ps>}*_3_rCC%lKnWD(%!*|0*;0~i~ZQ91)8~@RE78PM97xwEbU0O
z_BQ39msk#;K3l1RsgiU#kCr+0(|w=VrNXZ!Nfp;vP4FV<)X|%lpBr_=q{>$u$R+yT
z*5Y*x#ki6tyN<XUkvJC|e#ai|f|J`kBzZjX7!iuSu}d@Z-YVb396Wd03vdr4uuJlt
zMElPbiPmTC1|>ulZhn;=aSe$81zFFil_><4!bHC15HBE8p413tzcYev*;n^)1(P1=
zFN(5ndieCyXMQcdgdJ}_vx%hl9+hGT6X`ZJ7-W$_f#99Vbm>-t%?o>YnjxtuZFKVi
bT|`MO3_f$R?E&KlDL_ZdP!oI4?$7@L4o?$>

literal 0
HcmV?d00001

diff --git a/flatland_server/test/benchmark_world/map.yaml b/flatland_server/test/benchmark_world/map.yaml
new file mode 100644
index 00000000..ac269fbf
--- /dev/null
+++ b/flatland_server/test/benchmark_world/map.yaml
@@ -0,0 +1,7 @@
+image: map.png
+resolution: 0.050000
+origin: [-16.600000, -6.650000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
diff --git a/flatland_server/test/benchmark_world/map3d.png b/flatland_server/test/benchmark_world/map3d.png
new file mode 100644
index 0000000000000000000000000000000000000000..acc07667deaeca940e8538d11119969b22db65c0
GIT binary patch
literal 7569
zcmai32UL^WvJRjiDj<p=2tkO_q!(#{cn(MeDH5cJG>Ote4}?$xs31*~gEZ+!Y0`Ty
zp$93U7wHfoQbXvS$GPX;ckjAuz4g}G|N8%#Ju`deo7sEqS%275T`1FKuFC)bfJyVQ
zIt&1~2m}CV4lbQD0L@C6lJgg>^%JN%;4B2!jX$RtT^~P30svQT{PoZP5|VD6GZ{QI
zbsjOy(y-j%kbIlXHwpmo@N267_6$9~HszF%Q&U0T_bg?K&-rjBh}%bY<rMIY?b=O*
z@CP?yfg!KIH9G#yHGc=%S)+<efL2f-`oc9Fz-1%#(Zeh-GRu^BW`0_5w7C7bJl)hm
zYhk;5C|NW)ZiY~Fw;;A-pWs}qGnJh_F4pJIxW`LqrnM;jbu@96#lWHRlEcBWfa=yJ
zB7WajM>vBQTHvk9lZr?!zk2p0D@Jj6pm4E8KKm-oizjDi4L@S}yjB$Lk8Yy(U+%9v
z3o(TyYZPOsw0gaGZ`IfYSXhO+LsyYi=E$69yoYb)sbc3^>dHcvzr|v0Xyc1W<H`B$
zv%#KR^iRD(H~e8<>bHY-NUVeAnIV^M>-=yb|H6u8ivLp;ex8{>qDc!r;o1@VN2-sh
zp`Lq@<FB?2ESNh!eH9~j4h$Ss__5*YB!D12l+`Zl$1UpT_izv`H8}HzVyw$2c|=+%
z^;d_SJmRb#bug>e?os9!1wNtQoTtxPg{MjSeNsH#XM{WXPWhyHoxW-V$>-k{{pgu%
z9gv6gh*&3ISA66z%u@@+9<LH2$(BC;i#oi;hM~|3aZZ*ftZ>^}DnE4nx)9!EyA_em
zRybr1bzwR&>U}OF#3^r4+Fs2b2~)oc2^!g8YWuzNLMy1Qe6vF_WAg0P?QOZX-$$z7
z%d-K~%JWtn$)|l9v46~L^W^~NoR-Eou&lyakDn{}8`5O^1j%c>PJb+%inF$5P-$>b
z&hiP`i~FSb`@;U~j5Ku69vN<|{yt5{veJC;+~5$8@%`}wHpS!>mxDW{%dFF>apW&5
zCDcS!S?VRqTE;?{?(LcW>Rq>{31b<?>O>q%kq=Kja&7CKeBo=<gJm<isDsbs%QqVS
z^pP!MIQ<jJlMTVN|9<d^h1dv0`;u!CZ%C7)LBSr!IXa2Utf&0EJ3088LxzW7+E6Hf
zdZ~z}rI{wSQ-gZxPbAY{!{-7TDjp6E{61ZdzEGt_5Czh#?-tYCB7a3+c&hTZ(j6d8
zE2t4NpM(Gm|Er=q+s^A+{%geFmj0F);ZC6WoX_0`3M9(?UHJ2?|62GCu>X3aRugm3
zBfx($&kg@eQj8{#?H_aQR>W$0>ZQLm-Vo*fay&E^V00Nk?O-UOu>MoUj?pS(mS@%X
z!rzmDS;_|UtjUPadAHj@hpbi^fO7}+N-3;XU!2)+fE?(01;fLgj#s{bXE=ZZJs=tJ
z&q3|Lujv53H$eXm4jm5~1zhn=`$zcSfHddQlw3ZVb7TL9-+xE=?>PQJ2eMF89|0r{
zALIaO1eAy8S*@f5XdVkQ8CSF805S(c=iDuQFs=6W|Hmw~V=nlgh=F-NoO`fyj>~_5
z_HTIqX8t$*-){dm{a<eX?}_z)$NC@32aq5UfpqM&ZiQN*-)o^-!V5*RZ6Iz19k>X@
zv!WHWYM9Pl$rl%5Ekzhg4e=$a3o<OZHx{%=x$`28&DVa72LIYbW~M}k&$FT?UlX@h
zeoOU_1esHOyM;446g3|wBgPO?lE%#H*kJ6ABA=MrFle<DnGEan7FL3nMNxMj<Uof(
z+~SJJt8K5lJ(>_&j}-ZUFH<oY2{5hK1tob-I`=#o>@u6BGE7C)&#k46WAbGJ#Glk}
zmiSqo`=_Y#1jfS;RIK`30L%c*N2(>V{D!gp3zHy2m6d`5(JiljDm%_~Rs%m&Jwh3Q
z1%BTwF1~iIX%<hh;K;qKie%Pi;G$k)i3^PoB;EnOq%_@nBHz*5Dl*TyJD#sPV=b<%
z*82k7JpH7@Bu|2D6t>z|jt;`SsJqkrd$1k({D8pJzMyM|*)Ppjpo89J!k?SoYc>S|
zU*eb0Sxuw0KbH5;<;@)ir<Tv_J|qulsoH2gbFRiyV|m-C?cRJOD94NC#mslS<gPxr
zSv8lylu%0x1JfEr<NnH_=>@CcrYt5e#maLp4VrElG18wszaS}oYg0iI`LiRkq!R{y
z8^|4yXrPJ-A<A(zOEGcO6l7;UL!7}qMdqYV_9r#HGW$+*{jl<o)B0(*bzKor&zI+b
z?#_HC@@_CA53{-yv!a)IJqjWKHm{>T;GZwChZ6?lx*md4MGVef)gK3n4O{xLrzT<R
z&)DJl>2V2<vhSI97yIr1S(Z^%s#NEjocH>XQf$~KxA_VA_g<$#%d*N>%h7h3+89gQ
ziSMmBmWgvnqHspZHI~;Ekt3cGl9+961GUt=?TN4mujdjju4iIq;?1xpYYpkJCpj6h
z(GGhN5=={#WKwvq$R?g^3Vue(I>_cQ4|Tg=?U?I5)OcvdCBI-M2;<Ym^=B>~KH#9T
z59)w6VNuRDT*meJFA=Hg<j)duF|EF|q*?)q{UG9yvSpjK!7?P*KkB9zo#=G+)zgEJ
zNXWvYRy^jg3xj)Y9~{>IHp14p#*U)eQuWDXJX{{B7gKm>kP(2)ifAO;J#m;H3bPN<
za!gxJ&s*wntG#y|wev;vBwHN+9*y|u<-C^KSNc&6a*|B+Oh-Amf3h=3IDT?_E%wSC
zXaIA|$pM9fw<<XSsC1~ArZ+d1K9>2mPDsD2$Moij|K4fsdLnMDZHuwK2BO)nw0h05
zo47yPb%%16w$FUV^pQWaru~L+RYh##v9GTIaQZCBLYokV$NB$2N#sxEn3td6<r(gf
zcAwaV2jpUN4%c6Op6F3O-qMNHpej;LmbC?Ui&^aXtzwVj0{RZjqOcReaHWq)gUyiY
z+W}N_?PIwa9`5gvIRpW{fW@ub{k!YKZ*AW{9vWUCbuBEfb~zID0(h|aUh(!Bhz{n#
zj7@=^YgWP_KlsDTYb`^-8tCOeQ6le_tyQ=RpPA(T{>$R+;y_uple}4yLpx92HCKHV
z)ncQWAxuvX)3~pfgM`LOUcy)ZcQuaN>X9&qai_Q}zvE524<qig(m;3wTNP)Cfy46R
z+nRmzT4%N7ES6slKoxl@M{}!57vi3Vq@DEP^*xcQU4MIyg+uDr_ydka^|@BKWd=Mq
zn^!;g*wzWGidrhKX=r<7uwGVYvUO-7#@0J?cdp}v-uNc2&Pc^#rto>W9C0P}c(}Au
zV4#>4n>T){3~srm#?q#qCv;4dgR8e%mn)RDz{cGYy9<qQYV3<P(oMl6z8Ft=#x{^d
zTkq;g!eX&FIyDXHByHj6`6W_Dgo)yJyqE-MyPKG-S2@|$@I%`&<@=ZgZ&H|T#qoVL
z!=-ONW{$iQ#y9+2cE(hxUkDzF4%0s>w&*DK4N_MKB%hrk52F{rC-S0TysEgLjsmt<
z_`^-y*hRB|briVfI#@b`e$wT6LN7Ik0IkXQe5veL>A^jfy*T`0w?`Llq`29s0h>8Q
zQ%zFrjSs>+k~GIN#QoI+ww0~lWr}ipE2)%r6>^vQm`ypUa9A!J0-Z_)c)GOjui89J
zntA|rbem2ah~zjL$svH+N#h=7-+B^$bh^&zl1<AThmi3Q5fJyAxY|E+Yq;GCwUfX-
z8#SPS27TeZ!~NpLaw1R6^P5;3hoNyYg^SsYx3nqmK9D!JVbjA>IdiLc#calxhw8Xx
z$%)++U2E_<*1qTJ{DP~4hE%+GY|JFJsU60SKuJtv;tyhwugWmfhCU^Gy>;eOO!-Tw
zA)-3HCyx49mS7-nf*;gBf_$(=sfW<>11+7==3Ti+rj5z^(<p?Es=K<zNoc+x$LT}~
z1w1ZN83p14K>`+=NG-(iN>!z!eDTEZ>;n#>^VFq3?Og(#owukSH(Ox56L%rwcX_*B
zGNmYIq=81~sRv@~-4cb&jL>zxdaT)Wf)>4!JCD0(F}SYuBpOSY6mh<;QELBe@YpQ@
z{X$9AO~E2%-Mnsqebtu@mmAH{Ont!H`>th%1M4Nh$<OU=SyhsT&bzO{W+6c80W41z
zGz4(jG=1x>H`0kGx2#RR)$-3ivA1Bz0UE0t%A}K8zXwEwP_;O9uK4G0b|IPkszLP?
z&?$w0joFj*!|}U<O5=o$na*#WQc5%%%eXYZ`lM~tB6qymbQyhX!me_?aYIn|B<A9e
zX?s$n!BS2z3e_FFOtqq#Q0UcuCLJ;+Z%&ofdU>y84N9y#jf{c~qcWj(K?gb5v{oH>
zr|?3D@Gy*j!PL6?_@^Yb3iTPW?lo&RwNKug<_U1wS`B+sPTgc8Dl>lQS9rxx%RE5=
z_g5f)Ccv`qK^+8c+qv<zfZE|=djldwgCj=+CRM6XVQNm_ej{$8n3(k?;>J`^5=__o
zu!FQ<z8a`#y@Q7M&L0`QCzKS&Bp(%U8)$Ld&}jRepxC>Yz80uxmZoWmqHblG{z{He
zQHZTST6TM<@rvQ6el9`6VHU?x@=;v=iM=;o=-GpC0!twmZ%121`{d*_#bbK!ku3ZD
z#>m0VVn10p4t>0_-oIQUWy5tetj+mc6`1XH;!3)VK~VP>h>1I}#9${sUEv|XaT9xj
z1U5>-Cw~pT=Cw3#I#uo@i<(s!CJiPYh>E@`%m>aKSWlSt*wJ%;@$oC_nwGWcWN0M`
zan{bwJ89jBx(1l1Dm$XBpCrN@U{T_@y~j|i74-2*E=R3qV0o66BdpW&7Dq}l2aO+M
zXWvu-1?NU4^+wri2tsbLGn-|!(@dMM3&OkPT}%VG#?*H9J(|3wV%lP5jA#kpfFB)b
z6rPf~9)g|wSSv-(TZ215@(fBhTLpJnzLryRbC5Sm*jG6z(KZXYN?!afmn?9FLaY?;
zWpO<_*$j^1CWho1eodj@9V*F{XBUQrPkuykiyusrtyAeD5n4_hf1E7TuGs^KD1~*Y
z_L=0Y`j6EUJibsmNlOP$p02dP>I(v-{vuZz#$Qiq!Pjv!*P<24^5Tq%muH(@?G~@l
z@BY*ecR6)$39@N?1|HeG*zy$-Hh8P8&QqgP!%7;6CpD_8m`mgyjmC>NCi>>hJX0Cg
zH#@xWMxH<-pWh&}r?HUnjrpNSvtL}FSIq*v8we{buk64Z$<#5PPhrj_Vb1Mg6&{1K
zSgaC+BU-nbzTp9|>#5kH{~y+>hB(EAb?X;$at3N3c*o!*Mwxp+Dwd&&17|s1_+gUa
z_0meWbiu_fMLpyhUSWz-lgRAKinDzBs4W??_$y0%1;sz@tLAdsf_e2m(95;&!Tap4
zMLLRLhDSopuI+K9r4!BPa02MSd$zLC?Z+H_(K8kHquK2enqg4m@^U2IVf>*c^=i&}
zM?y-BkhsVZ3Kf5+FY*@svy)Qg!xnP~sL9XD-13F<A6cWZ@c4$oRfaqRw5C~IXiZyY
z-Oojv#R$zv_bnKkGm3_9#pD4#=L2=dXgAPgAIt4@8mfY3@YTFNif>-*g>!T}p@8HS
zD7Z8gM&?23%X5v<6`828bGTnT2aY^sxvW(CQ?7BLf$+x%fUWm2Mpsz<ZsQ}chBLzR
z8~v;ja3TTr2?sEynn2G0+|$EF8=LL#WV%3kA2aAx%aBu9AVAu3@z5t&I-9VvJErQZ
z(Gv5MVR#g*ll8qS?S~w~ToUKpCCLKdUp*Fr)X(?tF7qE<$ngYP)_|q)^Ofp`S;2ec
z`|r?Zob2yFK^lAS1Ac<I6@dYQpleoVRJ0PJ{ey~xVULLB7;XYI`lD&}`^6ZFxS|z~
zs=!@M9F|(OFL1fSPzcQB<TWN|JOUg&P?a6@f0{*|*1uDG5QxtM{*@N%X=tE6`H(H5
zO;;{3V!0S%!NZE}1NQay;%Lr_CadYsdo0m|Tu$HZsX&R3gz?Pi^X=J*Ql*`|y4wGP
z{@g+We{j%+TNGbM0ooh08&s3`80^`r8B!1D5Ox7@rZpNy99<goZ_kawHbP<vkTML*
z3nf~mQ9-Bhp>zq(A8;j^*~0l^f33Ny0<;IFF<)MaU8uVa{$s~(gZH)2#+?>4fIXKW
zjqO&6{xl_LZywK%76umDGM*i`$UwY&+xz3%b@>@A&XYIE9EUuORdmytzg(vbM1-^H
zyX!917I<JFyzsO~7r0B2Q-Ycko@Hm=Yx}hgB4%83Hx%1e4!*fChZF#hFt{F$#U|LN
z8dv5GhaHo|b%4ee-_kxJi+_WYy6-kxIsMHS<D-x&yJ1E?+`1$?3!10W;f&5su>pru
zqLN6IB$vrV^=Tii^qB?wC%kaI<Z%4s3n`2rH{%P%A7Ik-5$ftgFSnH&!<XI6ub+@q
z-by6%noI~lUo6wJ46D4IN-IJ@$_s%r;3Tp|>b}>UGu{hmXQ;jKa>#dll?ieLQ!QnA
z87lN)B<!&N2q;KeM78#gKhDYT<woZ@Z>Z9suHD8T{pzKir}i9FN%O2dr)y9K8rLUT
zmV-9@c~N#YYa3sA=Pq&I2d1uA(H%bmH-KZtARolzVrVL^M3d@<=LhM5SVBn@s5anG
zynF0Q4gpiw+aG*0JW{o>cwS24Wa+DyIcD?x+bvtbF|GO=F!i|+Q9Fp#20y7(te(@J
zz4HA>*;)kDwmEykwQ*UB;@{Kjq_~%zx@;DLyfJrv&l+GI@l)o0BbY-N!zi^!bUns%
zF>~TP@h)r84%ObMUii<^`J4Nj#2vCx1rkyy!qJ-@>sj!I!EZB~t@!z#!UEgUrs?H-
z7T$b;#GRCBJMo1BEj9%QFz`1a+VhPbgLSG0-b_iRr|Uj)Np-oucabE_jnv(7;Qd`!
zAoKz=2X1P8=s|`}Xmi+9Zsbnon>5b?k_Z#rdKCs%WVgU^x02-X`7hm5Pz@D+K4IUX
z3PxllLJF#Nhm!8y1-DIhSAc=n3j}I3+e><8iAnA`*-o%SPq~W<8aq`-NHymHYPGeZ
zyOxd@IrZ6@-j%mgo#fSyS|8|r=iSNM*icz*gMPb*MMr2+R^RvmQ5%0{RS$E-(dH)^
zZw2lQBu41AS&n34E-m({%KDD9fp>A-@geEV;kTlrWvN|GoRoo*^X6F1h->Jw6wi|5
zA3i}5P{3_N+|f*A1$E-e;7OxY4x#2KViGst_HJBXp;=#PbU4`L)L!bcp98rj{V|n@
zEC_HxdLJFMc28e&1mXhxv)}kJmoj<zc0#STh&z^h9KBIuT;@!E9y|MF-8rG#Z*YY`
z{Au<*v7!)kw+s4kpCoVV!Sc%tpX*h>7Un$)wXv4&vmV?lN-Ql1NP;k@UdNS=zVZGN
zov7fBDM!j|qF6b9M7k?H1D~3+_LsT%zYQa)dJnF=cmh>;2tLhK`WEY@>Z5(nr0o9U
zYPID{r6n%ZdTPP^3(JT*BWzxtI!YFkp5hmg?soL8B(I;O9B%<@r>y;LyDxC2c`79p
z>@r!7jnArX_`dxbVJmKP8>h;92WZ*dKB<$wyG!PTKnJ@e!uJz080w}Vx5uJFq;QnN
zMh7h?<ZuRFnfB5(+*zJO&ywEa=hc)~(YV3igm~VtECEmTx#d!K+yj#dq%Qa=e@1iy
z>~vNPbuf0A_ok9SdO-R@>dl%x8K=FqA40vQH?^`P*i)^%ba#{?Wo}8ZUWiv-Y49Ek
zj?^c&ssc<5G1`hlWIA_vg1N-?Z1oiv;B{Poc;6BnQo(Mr?gDWvDQ-Lxo)Ji(t_fC%
zvQnpaGxA{gzVss7)TKKdsGzA8`dxL^!V`-49lf+NWKMSMSPfZ~>Kch~sL6kr{p5S)
zI))rw<0m@KvZ@M1f-6`yV-7H1yW&iz%>vCm{E_&h;)*NAxtL#K!=rI@8#k#YydOe2
z4ESIU<Qq25^i)9`cASSePi->pfI3cHzS-^_`c&)(+eFb8+Hu-Ygy^8lkZ(*Ty<57~
z0^h^Uh+jN{^&ZyDitZ{m^Zl&-9tCoWG?M?a#i6_|ZsFr=t1PLUOX|6K<7@RgobKo&
zA;W)VzM&=w|BARO&eL@a3tbW>KHv#tT+Q{Vn|X&(Gb?lPPSojMe387=2i5K_SqYWn
z76c;dJod<2#ophnFskRLjbBL>Q<HDTmYv4bAeKgvu@*;I45lq=Sb5}>Wj`?_+hZN5
z>&!<~Sw|!M=~5@EQ*I^I7xbDTeC9r^Nkg`*rz^fW@}dpzr<8CmKNh>RJ6)afxJ?e>
zU!WI#egT?XD{>T&w&x>kUWtOqc1ip?J*po!lTBb=okCB-b>(8jcdd6^2?Vdk;h{`Z
zSeV>s_;h;K)a2A}uJRNCW+#q%$FTsilBGusQmbFqez5B+R;1{$@bI&v@4}0;_xB#)
zIw!pJIe2}l?&`+jTCO-)tgX{o>Z`QdZ6L8;NkTH*Kd~Jx-rjOC!G3j$$_q|yri0@5
z%+36xLS!7>%ZSzQLC<g;d{22l7l>wUSvAWgFFPw0D|p$vl%)YF!&4;lh(F_R)A)O~
zBn?VllZKHxu5$nBlV{#Ers~tdH~wry58j^{?I074SSrSP8;>+(xJ(U~3Aatgk4|l&
z2D!}Z-eY)Dq^jw)jc_NX5a8{q<%e#i{=nodXCj!JXix0*^r?7oLiH>iuYlOwEVjlc
z)h1^~^7ALzFcETqf;m;S*>MKxL*S=pI}7Li(BbnYt#t0Rc+QYMr?V12ZKr@xVLg@K
zk_tj)*dlK`MSfCK-ryOAH;BZ~mu)T{=q}~hc8u=4>{PVG(ubS)ethTErkMyM3KNDi
z><*1BBsiXKxgXq0>)alImR+ziQyKZ>uGJsgV3u=F%%V+_#dyGUuoCDzpm)vRO}l1|
zIf~4{Z@Wb+JsJ&5<R`E5EKJB;I!ewgHf(PFrXW7Jhr_Fi`X^Kfxu;L61Yp<c!jt&-
z0;({|d;S~;YRf+db$_m(OrHI@d0-iHO0&|Q>L%@(>%fCiR#~l$o$FeVc<)=7EM1Y_
zaq{{F|H9zl@X~~MIjMRu%{YU!)YR)DKVbi9a$VpaY$826tscy^vvs7sB!~CV5(_Yr
znfd7x*HLYmFcm<4%w4h-Z$F`@DFfbEc-K10Rv=Zotr3xZ_60AgfAvMm*3ml0irtmZ
zA<8M0x@illlZ!*+f*cb$eF6Rx(vIZAIE8@ne#pKy>Chmw>rr*E4-c+O-^iswOoI4W
z|NS?iu52lFs0zlH>XKckootXbJv}6*g}?Z#VK=pr()$`YwqK=FT>vs>lY;bu`}O9t
z{mP)$1P7nHK!~~Kj&qEnxNBT<y@#R+<?d*&3qC)G;s>X=FlO5L7FhoVmX3RlSDQ10
zY~CA*Pf#BI!eRLY&pHO?MPz5huBX=G8g#&TJd`5~GjouLy6o44B5%dM)|sca0S<M}
ze7zZ3LhhZu>PUvIE7^}i&?;+;-uh9QsDSnX=B$pdtCUiwI^SwY^C-n@ZEwtC)osno
zCMAFydzCpc&22&TEDW21Iz>X(u0kj}l$tjSUuRiyeuhD#TmCp{eNW&6`r7<n*H?x2
zCv7V<B#f5TVz+&pC89*V;B1>qp%zywIp4Z)hYOphzXTSDk3-CqRGuye0DG?V&s0N3
zV#Wt%?(Yq{lnZ;(eM=mCy+0nI<LEGG@8>Dw3+z0jL-2Vl7enJ8oc}-tXg<<aFZkW+
G?f(F~2KlxC

literal 0
HcmV?d00001

diff --git a/flatland_server/test/benchmark_world/map3d.yaml b/flatland_server/test/benchmark_world/map3d.yaml
new file mode 100644
index 00000000..6cec7384
--- /dev/null
+++ b/flatland_server/test/benchmark_world/map3d.yaml
@@ -0,0 +1,6 @@
+image: map3d.png
+resolution: 0.050000
+origin: [-16.600000, -6.650000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
\ No newline at end of file
diff --git a/flatland_server/test/benchmark_world/turtlebot.model.yaml b/flatland_server/test/benchmark_world/turtlebot.model.yaml
new file mode 100644
index 00000000..60be6a62
--- /dev/null
+++ b/flatland_server/test/benchmark_world/turtlebot.model.yaml
@@ -0,0 +1,53 @@
+# Turtlebot w/ lidar
+bodies:  # List of named bodies
+  - name: base
+    pose: [0, 0, 0] 
+    type: dynamic  
+    color: [1, 1, 1, 0.75] 
+    footprints:
+      - type: circle
+        radius: 0.5
+        center: [-1, 0.0]
+        density: 1
+
+      - type: polygon
+        sensor: true
+        points: [[-.45, -.05], [-.45, 0.05], [-.35, 0.05], [-.35, -0.05]]
+        layers: []
+        density: 1
+
+      - type: polygon
+        sensor: true
+        points: [[-.125, -.4], [-.125, -.3], [.125, -.3], [.125, -.4]]
+        density: 1
+
+      - type: polygon
+        sensor: true
+        points: [[-.125, .4], [-.125, .3], [.125, .3], [.125, .4]]
+        density: 100000
+plugins:
+  - type: DiffDrive 
+    name: turtlebot_drive 
+    body: base
+    odom_frame_id: map
+    pub_rate: 10
+    # odom_pose_noise: [0.1, 0.1, 0.1]
+    # odom_twist_noise: [0.1, 0.1, 0.1]
+    # odom_pose_covariance: [0.00, 0.01, 0.02, 0.03, 0.04, 0.05, 
+    #                         0.06, 0.07, 0.08, 0.09, 0.10, 0.11, 
+    #                         0.12, 0.13, 0.14, 0.15, 0.16, 0.17, 
+    #                         0.18, 0.19, 0.20, 0.21, 0.22, 0.23, 
+    #                         0.24, 0.25, 0.26, 0.27, 0.28, 0.29, 
+    #                         0.30, 0.31, 0.32, 0.33, 0.34, 0.35]
+    
+  - type: ModelTfPublisher
+    name: tf_publisher
+    publish_tf_world: true
+
+  - type: Laser
+    name: laser_front
+    body: base
+    always_publish: true
+    range: 20
+    update_rate: 25  # Hz
+    angle: {min: -2.35619, max: 2.35619, increment: 0.004363323} # 1080 scans, 270 deg arc, 0.25deg inc
diff --git a/flatland_server/test/benchmark_world/world.yaml b/flatland_server/test/benchmark_world/world.yaml
new file mode 100644
index 00000000..887e29ad
--- /dev/null
+++ b/flatland_server/test/benchmark_world/world.yaml
@@ -0,0 +1,16 @@
+properties: {} # For later use
+layers:  # Support for arbitrary number of layers
+  - name: "2d"  # layer 0 named "2d"
+    map: "map.yaml"  # leading / denotes absolute file path, otherwise relative
+    color: [0, 1, 0, 1]  # List of floats [r,g,b,a] to color debug boundary
+  - name: "3d"  # layer 1 named "3d"
+    map: "map3d.yaml"
+    color: [1.0, 0.0, 0.0, 0.5]  # red 50% transparent
+models:
+  - name: cleaner1 
+    pose: [12, 0, 0]
+    model: "cleaner.model.yaml"
+  # - name: turtlebot1 
+  #   pose: [12, 0, 0]
+  #   model: "turtlebot.model.yaml"
+    
\ No newline at end of file
diff --git a/flatland_server/test/conestogo_office_test/world.yaml b/flatland_server/test/conestogo_office_test/world.yaml
index ad143c78..a0d1d544 100644
--- a/flatland_server/test/conestogo_office_test/world.yaml
+++ b/flatland_server/test/conestogo_office_test/world.yaml
@@ -16,14 +16,14 @@ models:
   - name: sensor
     pose: [2, 0, 0]
     model: "sensor.model.yaml"
-plugins:
-  - name: test1
-    type: RandomWall
-    layer: "2d"
-    num_of_walls: 10
-    wall_wall_dist: 1
-    double_wall: true
-    robot_name: cleaner1
+# plugins:
+#   - name: test1
+#     type: RandomWall
+#     layer: "2d"
+#     num_of_walls: 10
+#     wall_wall_dist: 1
+#     double_wall: true
+#     robot_name: cleaner1
 
 
   # - name: turtlebot1 

From b5da25b43ab7b9c6a266cc69084ab2ea5bc114c5 Mon Sep 17 00:00:00 2001
From: Sees <Vsevolod.Egorov@ronavi.ru>
Date: Thu, 14 Mar 2024 14:29:27 +0300
Subject: [PATCH 5/6] added the capability to work with non sim time in
 timekeeper

---
 flatland_server/launch/server.launch |  2 --
 flatland_server/src/timekeeper.cpp   | 13 +++++++++++--
 2 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/flatland_server/launch/server.launch b/flatland_server/launch/server.launch
index a465b1c7..c2651565 100644
--- a/flatland_server/launch/server.launch
+++ b/flatland_server/launch/server.launch
@@ -13,8 +13,6 @@
 
   <env name="ROSCONSOLE_FORMAT" value="[${severity} ${time} ${logger}]: ${message}" />
 
-  <param name="use_sim_time" value="true"/>  
-
   <!-- launch flatland server -->
   <node name="flatland_server" pkg="flatland_server" type="flatland_server" output="screen">  
     <!-- Use the arguments passed into the launchfile for this node -->
diff --git a/flatland_server/src/timekeeper.cpp b/flatland_server/src/timekeeper.cpp
index 8b27b597..77514464 100644
--- a/flatland_server/src/timekeeper.cpp
+++ b/flatland_server/src/timekeeper.cpp
@@ -49,8 +49,17 @@
 
 namespace flatland_server {
 
-Timekeeper::Timekeeper()
-    : time_(ros::Time(0, 0)), max_step_size_(0), clock_topic_("/clock") {
+Timekeeper::Timekeeper() : max_step_size_(0), clock_topic_("/clock") {
+  bool use_sim_time = false;
+  nh_.getParam("use_sim_time", use_sim_time);
+
+  if (!use_sim_time) {
+    time_ =
+        ros::Time(ros::WallTime::now().toSec(), ros::WallTime::now().toNSec());
+  } else {
+    time_ = ros::Time(0, 0);
+  }
+
   clock_pub_ = nh_.advertise<rosgraph_msgs::Clock>(clock_topic_, 1);
 }
 

From 33beda44348e39ad707b4c797cc1843ec6437c20 Mon Sep 17 00:00:00 2001
From: Sees <Vsevolod.Egorov@ronavi.ru>
Date: Mon, 24 Jun 2024 22:49:45 +0300
Subject: [PATCH 6/6] MVP: fix for use sim time in simulation, part 2

---
 .../include/flatland_server/timekeeper.h      | 10 ++++++-
 flatland_server/src/timekeeper.cpp            | 26 ++++++++++++-------
 flatland_server/src/world.cpp                 |  5 ++++
 3 files changed, 30 insertions(+), 11 deletions(-)

diff --git a/flatland_server/include/flatland_server/timekeeper.h b/flatland_server/include/flatland_server/timekeeper.h
index ea2b81f2..498357cb 100644
--- a/flatland_server/include/flatland_server/timekeeper.h
+++ b/flatland_server/include/flatland_server/timekeeper.h
@@ -57,8 +57,10 @@ class Timekeeper {
   ros::Publisher clock_pub_;       ///< the topic to publish the clock
   ros::NodeHandle nh_;             ///< ROS Node handle
   ros::Time time_;                 ///< simulation time
+  double step_size_;               ///< loop step size
   double max_step_size_;           ///< maximum step size
-  const std::string clock_topic_;  ///< the name of the clock topic
+  std::string clock_topic_;        ///< the name of the clock topic
+  bool use_sim_time_;              ///< use fix time step or not
 
   /**
    * @brief constructor
@@ -81,6 +83,12 @@ class Timekeeper {
    */
   void SetMaxStepSize(double step_size);
 
+  /**
+   * @brief Set the loop step size
+   * @param[in] step_size The step size
+   */
+  void SetStepSize(double step_size);
+
   /**
    * @return The current simulation time
    */
diff --git a/flatland_server/src/timekeeper.cpp b/flatland_server/src/timekeeper.cpp
index 77514464..323e2202 100644
--- a/flatland_server/src/timekeeper.cpp
+++ b/flatland_server/src/timekeeper.cpp
@@ -49,24 +49,26 @@
 
 namespace flatland_server {
 
-Timekeeper::Timekeeper() : max_step_size_(0), clock_topic_("/clock") {
-  bool use_sim_time = false;
-  nh_.getParam("use_sim_time", use_sim_time);
+Timekeeper::Timekeeper() : step_size_(0), max_step_size_(0), use_sim_time_(false) {
+  nh_.getParam("use_sim_time", use_sim_time_);
 
-  if (!use_sim_time) {
+  if (!use_sim_time_) {
     time_ =
         ros::Time(ros::WallTime::now().toSec(), ros::WallTime::now().toNSec());
   } else {
     time_ = ros::Time(0, 0);
+    clock_topic_ = "/clock"; 
+    clock_pub_ = nh_.advertise<rosgraph_msgs::Clock>(clock_topic_, 1);
   }
-
-  clock_pub_ = nh_.advertise<rosgraph_msgs::Clock>(clock_topic_, 1);
 }
 
 void Timekeeper::StepTime() {
-  time_ += ros::Duration(max_step_size_);
-
-  UpdateRosClock();
+  if (!use_sim_time_)
+    time_ += ros::Duration(step_size_);
+  else {
+    time_ += ros::Duration(max_step_size_);
+    UpdateRosClock();
+  }
 }
 
 void Timekeeper::UpdateRosClock() const {
@@ -79,9 +81,13 @@ void Timekeeper::SetMaxStepSize(double step_size) {
   max_step_size_ = step_size;
 }
 
+void Timekeeper::SetStepSize(double step_size) {
+  step_size_ = step_size;
+}
+
 const ros::Time& Timekeeper::GetSimTime() const { return time_; }
 
-double Timekeeper::GetStepSize() const { return max_step_size_; }
+double Timekeeper::GetStepSize() const { return step_size_; }
 
 double Timekeeper::GetMaxStepSize() const { return max_step_size_; }
 
diff --git a/flatland_server/src/world.cpp b/flatland_server/src/world.cpp
index 0b316526..47f91aa0 100644
--- a/flatland_server/src/world.cpp
+++ b/flatland_server/src/world.cpp
@@ -100,11 +100,16 @@ World::~World() {
 
 void World::Update(Timekeeper &timekeeper) {
   if (!IsPaused()) {
+    std::chrono::duration<double> loop_start = std::chrono::steady_clock::now().time_since_epoch();
+
     plugin_manager_.BeforePhysicsStep(timekeeper);
     physics_world_->Step(timekeeper.GetStepSize(), physics_velocity_iterations_,
                          physics_position_iterations_);
     timekeeper.StepTime();
     plugin_manager_.AfterPhysicsStep(timekeeper);
+
+    double loop_step = ((std::chrono::duration<double, std::ratio<1, 1>>)(std::chrono::steady_clock::now().time_since_epoch()) - loop_start).count();
+    timekeeper.SetStepSize(loop_step);
   }
   int_marker_manager_.update();
 }