added the capability to work with non sim time in timekeeper

docs/core_functions/ros_launch.rst

@@ -25,12 +25,14 @@ 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
 * **step_size**: amount of time to step each loop in seconds
 * **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
+* **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

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

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

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,

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
 

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

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

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

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>

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

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>

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()));