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/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/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_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 #include #include +#include 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::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("topic", "scan"); frame_id_ = reader.Get("frame", GetName()); broadcast_tf_ = reader.Get("broadcast_tf", true); + always_publish_ = reader.Get("always_publish", false); update_rate_ = reader.Get("update_rate", std::numeric_limits::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 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/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/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/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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 @@ - - 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 @@ flatland_server - 1.4.1 + 1.5.0 The flatland_server package BSD https://bitbucket.org/avidbots/flatland 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; im_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(fixture->GetType())); 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 +#include +#include + +#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 e033dfc2..8c182f58 100644 --- a/flatland_server/src/layer.cpp +++ b/flatland_server/src/layer.cpp @@ -221,95 +221,68 @@ void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh, double resolution) { 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& 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 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(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); + + // simplify_map rosparam: 0=None, 1=moderate, 2=maximum simplification of map polygon outlines + int simplify = 0; + ros::param::param("simplify_map", simplify, 0); + + std::vector> vectors_outline; + cv::Mat obstacle_map_open; + 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); + } 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(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 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& poly_to_use = polygon2f; + + + if (simplify >= 1) { + std::vector 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 { 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 #include #include +#include +#include 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::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 start = std::chrono::steady_clock::now().time_since_epoch(); + std::chrono::duration expected_cycle_time(1.0/update_rate_); + std::chrono::duration actual_cycle_time(0.0); + using seconds_d = std::chrono::duration>; + 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 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 expected_end = start + expected_cycle_time; + std::chrono::duration actual_end = std::chrono::steady_clock::now().time_since_epoch(); + std::chrono::duration 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/src/timekeeper.cpp b/flatland_server/src/timekeeper.cpp index 8b27b597..323e2202 100644 --- a/flatland_server/src/timekeeper.cpp +++ b/flatland_server/src/timekeeper.cpp @@ -49,15 +49,26 @@ namespace flatland_server { -Timekeeper::Timekeeper() - : time_(ros::Time(0, 0)), max_step_size_(0), clock_topic_("/clock") { - clock_pub_ = nh_.advertise(clock_topic_, 1); +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_) { + time_ = + ros::Time(ros::WallTime::now().toSec(), ros::WallTime::now().toNSec()); + } else { + time_ = ros::Time(0, 0); + clock_topic_ = "/clock"; + clock_pub_ = nh_.advertise(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 { @@ -70,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 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>)(std::chrono::steady_clock::now().time_since_epoch()) - loop_start).count(); + timekeeper.SetStepSize(loop_step); } int_marker_manager_.update(); } 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 00000000..a29b0ba1 Binary files /dev/null and b/flatland_server/test/benchmark_world/map.png differ 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 00000000..acc07667 Binary files /dev/null and b/flatland_server/test/benchmark_world/map3d.png differ 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