diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 4f3af87f..80fb4a0a 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -1,13 +1,21 @@ name: CI -on: [push, pull_request] +on: [push, pull_request, workflow_dispatch] + +permissions: + contents: read jobs: tue-ci: - name: TUe CI - ${{ github.event_name }} + name: TUe CI - ${{ github.event_name }} (${{ matrix.ros_distro }}) runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + ros_distro: [humble, jazzy, rolling-u24] steps: - name: TUe CI uses: tue-robotics/tue-env/ci/main@master with: + image: ghcr.io/tue-robotics/tue-env-ros-${{ matrix.ros_distro }} package: ${{ github.event.repository.name }} diff --git a/3rdparty/AMENT_IGNORE b/3rdparty/AMENT_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/3rdparty/CATKIN_IGNORE b/3rdparty/CATKIN_IGNORE deleted file mode 100644 index 8b137891..00000000 --- a/3rdparty/CATKIN_IGNORE +++ /dev/null @@ -1 +0,0 @@ - diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e3fc489..22d5f20f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,61 +1,61 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(ed) -add_compile_options(-Wall -Werror=all) -add_compile_options(-Wextra -Werror=extra) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -# Find before adding cmake_modules, to not use their deprecated FindEigen -find_package(PCL REQUIRED COMPONENTS common) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Werror=all) + add_compile_options(-Wextra -Werror=extra) +endif() -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - code_profiler - diagnostic_updater - ${PROJECT_NAME}_msgs - geolib2 - pluginlib - rgbd - rgbd_msgs - rosconsole_bridge - roscpp - tf2_ros - tue_config - tue_filesystem - tue_serialization -) +find_package(ament_cmake REQUIRED) + +find_package(ament_index_cpp REQUIRED) +find_package(code_profiler REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(ed_interfaces REQUIRED) +find_package(geolib2 REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rgbd REQUIRED) +find_package(rgbd_interfaces REQUIRED) +find_package(rosconsole_bridge REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tue_config REQUIRED) +find_package(tue_serialization REQUIRED) +find_package(tue_serialization_interfaces REQUIRED) +# Non-ROS dependencies +find_package(Boost REQUIRED COMPONENTS thread) find_package(OpenCV REQUIRED) find_package(orocos_kdl REQUIRED) -find_package(SDFormat REQUIRED) +# Find before adding cmake_modules, to not use their deprecated FindEigen +find_package(PCL REQUIRED COMPONENTS common) +find_package(sdformat_vendor REQUIRED) +find_package(sdformat14 REQUIRED) find_package(TinyXML2 REQUIRED) -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME}_core ${PROJECT_NAME}_io ${PROJECT_NAME}_server ${PROJECT_NAME}_hello_world_plugin ${PROJECT_NAME}_custom_properties_plugin - CATKIN_DEPENDS code_profiler ${PROJECT_NAME}_msgs geolib2 pluginlib rgbd rgbd_msgs roscpp tf2_ros tue_config tue_filesystem tue_serialization - DEPENDS OpenCV PCL -) - -########### -## Build ## -########### - -include_directories( - include - SYSTEM - 3rdparty/polypartition/include - 3rdparty/rapidjson/include - ${TinyXML2_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${SDFormat_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) +# ------------------------------------------------------------------------------------------------ +# 3RDPARTY +# ------------------------------------------------------------------------------------------------ +# Exclude vendored 3rdparty source from compile_commands.json so clang-tidy does not lint it +set(CMAKE_EXPORT_COMPILE_COMMANDS OFF) add_library(polypartition 3rdparty/polypartition/src/polypartition.cpp) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +target_include_directories(polypartition PUBLIC + $ + $ +) # ------------------------------------------------------------------------------------------------ # ED CORE @@ -64,8 +64,7 @@ add_library(polypartition 3rdparty/polypartition/src/polypartition.cpp) # Get all the headers file(GLOB_RECURSE HEADER_FILES include/*.h) -# Declare a cpp library -add_library(${PROJECT_NAME}_core +add_library(${PROJECT_NAME}_core SHARED ${HEADER_FILES} src/convex_hull_2d.cpp @@ -90,13 +89,38 @@ add_library(${PROJECT_NAME}_core src/error_context.cpp src/logging.cpp - # Forward geolib2/tue_filesystem logging to rosconsole + # Forward geolib2/tue_config (console_bridge) logging to ROS src/rosconsole_bridge.cpp ) -target_link_libraries(${PROJECT_NAME}_core polypartition ${TinyXML2_LIBRARIES} ${PCL_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME}_core ${catkin_EXPORTED_TARGETS}) +target_include_directories(${PROJECT_NAME}_core PUBLIC + $ + $ + $ +) +target_include_directories(${PROJECT_NAME}_core SYSTEM PRIVATE + ${PCL_INCLUDE_DIRS} +) +target_link_libraries(${PROJECT_NAME}_core PUBLIC + polypartition + Boost::thread + code_profiler::code_profiler + geolib2::geolib + rgbd::rgbd + rosconsole_bridge::rosconsole_bridge + tinyxml2::tinyxml2 + tue_config::tue_config + tue_serialization::tue_serialization + sdformat14::sdformat14 + ${PCL_LIBRARIES} +) +ament_target_dependencies(${PROJECT_NAME}_core PUBLIC + diagnostic_updater + pluginlib + rclcpp + tf2_ros +) -add_library(${PROJECT_NAME}_io +add_library(${PROJECT_NAME}_io SHARED src/io/filesystem/read.cpp src/io/filesystem/write.cpp @@ -108,39 +132,52 @@ add_library(${PROJECT_NAME}_io src/serialization/serialization.cpp ) -target_link_libraries(${PROJECT_NAME}_io ${PROJECT_NAME}_core) +target_link_libraries(${PROJECT_NAME}_io + ${PROJECT_NAME}_core + tue_serialization::tue_serialization + ${tue_serialization_interfaces_TARGETS} + ${ed_interfaces_TARGETS} +) -add_library(${PROJECT_NAME}_server +add_library(${PROJECT_NAME}_server SHARED include/${PROJECT_NAME}/server.h src/server.cpp ) -target_link_libraries(${PROJECT_NAME}_server ${PROJECT_NAME}_core ${PROJECT_NAME}_io) +target_link_libraries(${PROJECT_NAME}_server PUBLIC + ${PROJECT_NAME}_core + ${PROJECT_NAME}_io + ${ed_interfaces_TARGETS} +) +ament_target_dependencies(${PROJECT_NAME}_server PUBLIC std_msgs) # ------------------------------------------------------------------------------------------------ # SERVER # ------------------------------------------------------------------------------------------------ -# Create executable add_executable(${PROJECT_NAME} src/${PROJECT_NAME}.cpp) -target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_core ${PROJECT_NAME}_io ${PROJECT_NAME}_server) +target_link_libraries(${PROJECT_NAME} + ${PROJECT_NAME}_core + ${PROJECT_NAME}_io + ${PROJECT_NAME}_server + ${ed_interfaces_TARGETS} +) # ------------------------------------------------------------------------------------------------ # PLUGINS # ------------------------------------------------------------------------------------------------ -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -add_library(${PROJECT_NAME}_tf_publisher_plugin plugins/tf_publisher_plugin.cpp) -target_include_directories(${PROJECT_NAME}_tf_publisher_plugin BEFORE PRIVATE SYSTEM ${tf2_geometry_msgs_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME}_tf_publisher_plugin ${tf2_geometry_msgs_LIBRARIES} ${tf2_LIBRARIES} ${catkin_LIBRARIES}) +add_library(${PROJECT_NAME}_tf_publisher_plugin SHARED plugins/tf_publisher_plugin.cpp) +target_link_libraries(${PROJECT_NAME}_tf_publisher_plugin PUBLIC ${PROJECT_NAME}_core) +ament_target_dependencies(${PROJECT_NAME}_tf_publisher_plugin PUBLIC tf2 tf2_geometry_msgs tf2_ros) -find_package(kdl_parser REQUIRED) -add_library(${PROJECT_NAME}_robot_plugin plugins/robot_plugin.cpp) -target_include_directories(${PROJECT_NAME}_robot_plugin BEFORE PRIVATE SYSTEM ${kdl_parser_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME}_robot_plugin ${kdl_parser_LIBRARIES} ${catkin_LIBRARIES}) +add_library(${PROJECT_NAME}_robot_plugin SHARED plugins/robot_plugin.cpp) +target_link_libraries(${PROJECT_NAME}_robot_plugin PUBLIC ${PROJECT_NAME}_core) +ament_target_dependencies(${PROJECT_NAME}_robot_plugin PUBLIC kdl_parser sensor_msgs urdf) -add_library(${PROJECT_NAME}_sync_plugin plugins/sync_plugin.cpp) -target_link_libraries(${PROJECT_NAME}_sync_plugin ${catkin_LIBRARIES}) +add_library(${PROJECT_NAME}_sync_plugin SHARED plugins/sync_plugin.cpp) +target_link_libraries(${PROJECT_NAME}_sync_plugin ${PROJECT_NAME}_core ${ed_interfaces_TARGETS}) + +pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) # ------------------------------------------------------------------------------------------------ # TOOLS @@ -153,93 +190,123 @@ add_executable(${PROJECT_NAME}_heightmap_to_mesh tools/heightmap_to_mesh.cpp) target_link_libraries(${PROJECT_NAME}_heightmap_to_mesh ${PROJECT_NAME}_core) add_executable(configure tools/configure.cpp) -target_link_libraries(configure ${PROJECT_NAME}_core) - -#add_executable(${PROJECT_NAME}_repl tools/repl.cpp) -#target_link_libraries(${PROJECT_NAME}_repl readline) +target_link_libraries(configure ${PROJECT_NAME}_core ${ed_interfaces_TARGETS}) # ------------------------------------------------------------------------------------------------ # EXAMPLES # ------------------------------------------------------------------------------------------------ -add_library(${PROJECT_NAME}_hello_world_plugin examples/hello_world/hello_world_plugin.cpp) -add_library(${PROJECT_NAME}_custom_properties_plugin examples/custom_properties/custom_properties_plugin.cpp) - -# ------------------------------------------------------------------------------------------------ -# TESTS -# ------------------------------------------------------------------------------------------------ - -add_executable(${PROJECT_NAME}_test_wm test/test_wm.cpp) -target_link_libraries(${PROJECT_NAME}_test_wm ${PROJECT_NAME}_core ${OpenCV_LIBRARIES}) - -add_executable(test_mask test/test_mask.cpp) -target_link_libraries(test_mask ${PROJECT_NAME}_core ${OpenCV_LIBRARIES}) - -add_executable(test_service_speed test/test_service_speed.cpp) -target_link_libraries(test_service_speed ${PROJECT_NAME}_core) - -add_executable(show_gui test/show_gui.cpp) -target_link_libraries(show_gui ${PROJECT_NAME}_core ${OpenCV_LIBRARIES}) - -add_executable(${PROJECT_NAME}_test_heightmap_triangulation - test/test_heightmap_triangulation.cpp -) -target_link_libraries(${PROJECT_NAME}_test_heightmap_triangulation ${PROJECT_NAME}_core ${OpenCV_LIBRARIES} ${geolib2_LIBRARIES} ${tue_config_LIBRARIES}) +add_library(${PROJECT_NAME}_hello_world_plugin SHARED examples/hello_world/hello_world_plugin.cpp) +target_link_libraries(${PROJECT_NAME}_hello_world_plugin ${PROJECT_NAME}_core) +add_library(${PROJECT_NAME}_custom_properties_plugin SHARED examples/custom_properties/custom_properties_plugin.cpp) +target_link_libraries(${PROJECT_NAME}_custom_properties_plugin ${PROJECT_NAME}_core) # ------------------------------------------------------------------------------------------------ # INSTALL # ------------------------------------------------------------------------------------------------ -catkin_install_python( +install( PROGRAMS tools/entity-teleop tools/list_plugins - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install( - FILES plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION lib/${PROJECT_NAME} ) install( DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) +install(FILES plugins.xml DESTINATION share/${PROJECT_NAME}) + install( TARGETS + polypartition ${PROJECT_NAME}_core ${PROJECT_NAME}_io ${PROJECT_NAME}_server - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install( TARGETS - configure ${PROJECT_NAME} - ${PROJECT_NAME}_custom_properties_plugin - ${PROJECT_NAME}_heightmap_to_mesh + ${PROJECT_NAME}_tf_publisher_plugin + ${PROJECT_NAME}_robot_plugin + ${PROJECT_NAME}_sync_plugin ${PROJECT_NAME}_hello_world_plugin - ${PROJECT_NAME}_test_heightmap_triangulation - ${PROJECT_NAME}_test_wm + ${PROJECT_NAME}_custom_properties_plugin ${PROJECT_NAME}_view_model - polypartition - show_gui - test_mask - test_service_speed - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ${PROJECT_NAME}_heightmap_to_mesh + configure + DESTINATION lib/${PROJECT_NAME} +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ament_index_cpp + code_profiler + diagnostic_updater + ed_interfaces + geolib2 + pluginlib + rclcpp + rgbd + rosconsole_bridge + std_msgs + tf2_ros + tue_config + tue_serialization + Boost + OpenCV + PCL ) # ------------------------------------------------------------------------------------------------ -# CI +# TESTS # ------------------------------------------------------------------------------------------------ -if (CATKIN_ENABLE_TESTING) - find_package(catkin_lint_cmake REQUIRED) - catkin_add_catkin_lint_test("-W2 --ignore LITERAL_PROJECT_NAME --ignore UNSORTED_LIST") +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_clang_format REQUIRED) + find_package(ament_cmake_clang_tidy REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_xmllint REQUIRED) + find_package(tue_lint_config REQUIRED) + + add_executable(${PROJECT_NAME}_test_wm test/test_wm.cpp) + target_link_libraries(${PROJECT_NAME}_test_wm ${PROJECT_NAME}_core ${OpenCV_LIBRARIES}) + + add_executable(test_mask test/test_mask.cpp) + target_link_libraries(test_mask ${PROJECT_NAME}_core ${OpenCV_LIBRARIES}) + + add_executable(test_service_speed test/test_service_speed.cpp) + target_link_libraries(test_service_speed ${PROJECT_NAME}_core ${ed_interfaces_TARGETS}) + + add_executable(show_gui test/show_gui.cpp) + target_link_libraries(show_gui + ${PROJECT_NAME}_core ${OpenCV_LIBRARIES} ${ed_interfaces_TARGETS} ${tue_serialization_interfaces_TARGETS}) + + add_executable(${PROJECT_NAME}_test_heightmap_triangulation test/test_heightmap_triangulation.cpp) + target_link_libraries(${PROJECT_NAME}_test_heightmap_triangulation + ${PROJECT_NAME}_core ${OpenCV_LIBRARIES} geolib2::geolib tue_config::tue_config) + + ament_clang_format(CONFIG_FILE ${tue_lint_config_DIR}/../config/.clang-format --clang-format-version=21) + # ed has many translation units; run clang-tidy in parallel and allow extra time so the test + # does not hit the default 300s timeout (which manifests as a "did not generate a result file" error). + cmake_host_system_information(RESULT CLANG_TIDY_JOBS QUERY NUMBER_OF_LOGICAL_CORES) + if(NOT CLANG_TIDY_JOBS OR CLANG_TIDY_JOBS EQUAL 0) + set(CLANG_TIDY_JOBS 1) + endif() + ament_clang_tidy(CONFIG_FILE ${tue_lint_config_DIR}/../config/.clang-tidy --clang-tidy-version=21 + JOBS ${CLANG_TIDY_JOBS} TIMEOUT 600 + ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_BINARY_DIR}/compile_commands.json) + ament_lint_cmake(MAX_LINE_LENGTH 120) + ament_xmllint(MAX_LINE_LENGTH 120) endif() + +ament_package() diff --git a/docs/migrate_to_ros2.md b/docs/migrate_to_ros2.md new file mode 100644 index 00000000..0b4d5643 --- /dev/null +++ b/docs/migrate_to_ros2.md @@ -0,0 +1,563 @@ +# Skill: Migrate a tue-robotics package from ROS 1 (catkin) to ROS 2 (ament / Jazzy) + +> **Purpose:** Migrate `ed` — and any remaining ROS 1 package in the +> `tue-robotics` stack — from catkin/roscpp to ament_cmake/rclcpp on ROS 2 +> (Humble / Jazzy / Rolling). Preserve behaviour. Make surgical, reviewable +> changes. +> +> **Reference templates.** Several packages are already migrated. Their old +> ROS 1 code lives on the `ros1` git branch and the ROS 2 code on `master`. +> When in doubt, diff them: `git -C diff origin/ros1 master`. +> +> | Package | Use it as the model for… | +> | --- | --- | +> | `tue_config`, `geolib2` | C++ library, CMake/ament target export, console_bridge logging | +> | `ed_msgs` → `ed_interfaces` | message/service (`rosidl`) package, renaming, type changes | +> | `rgbd` | ROS **node** (roscpp→rclcpp), nodelet→component, tf2, launch files | +> | `tue_filesystem` | **deprecated** — replaced by `std::filesystem`, see §3 | + +--- + +## 0. Migration order (dependencies first) + +A package can only build once its dependencies are ROS 2. The dependency +order for `ed`: + +1. Interface packages: `tue_serialization_interfaces`, `ed_interfaces`, + `rgbd_interfaces` — **already migrated**. +2. Libraries: `tue_config`, `geolib2`, `tue_serialization`, `rgbd`, + `code_profiler`, `rosconsole_bridge` — **already migrated**. +3. `tue_filesystem` — **do not migrate**; remove it from every package (§3). +4. `ed` itself — this package (§4–§9). + +Confirm a dependency is migrated before relying on it: +`git -C rev-parse --abbrev-ref HEAD` is `master` **and** +`grep buildtool_depend /package.xml` shows `ament_cmake`. + +--- + +## 1. `package.xml` + +Apply these edits (see `git diff origin/ros1 master -- package.xml` in any +reference package for a worked example): + +| ROS 1 | ROS 2 | +| --- | --- | +| `catkin` | `ament_cmake` | +| `cmake_modules` | **remove** (FindEigen etc. are not needed) | +| `message_generation` | `rosidl_default_generators` *(interface pkgs only)* | +| `message_runtime` | `rosidl_default_runtime` *(interface pkgs only)* | +| `roscpp` | `rclcpp` (+ `rclcpp_components` if it has nodes) | +| `roslib`, `python3-rospkg` | **remove** (ROS 1 only; use `ament_index_cpp` if package lookup is needed) | +| `rosconsole_bridge` | **keep** — still needed if a (recursive) dependency logs via `console_bridge`; see logging, §6 | +| `tf` | `tf2` / `tf2_ros` | +| `nodelet` / `pluginlib` (for nodelets) | `rclcpp_components` (pluginlib stays only for *your own* plugin systems, see §7) | +| `ed_msgs` | `ed_interfaces` | +| `rgbd_msgs` | `rgbd_interfaces` | +| `tue_serialization` (when used as **messages**) | `tue_serialization_interfaces` | +| `tue_filesystem` | **remove** (§3) | +| `catkin_lint_cmake` / `rosunit` / `rostest` | the ament linter set, below | + +Add the ament linter/test deps and the build-type export (copy verbatim from +`tue_config`/`geolib2` `master`): + +```xml +ament_cmake_clang_format +ament_cmake_clang_tidy +ament_cmake_gtest +ament_cmake_lint_cmake +ament_cmake_xmllint +clang-format-21 +clang-tidy-21 +tue_lint_config + + + ament_cmake + + + +``` + +Interface packages additionally need +`rosidl_interface_packages`. + +--- + +## 2. `CMakeLists.txt` + +### 2.1 Header (compiler flags + C++17) + +```cmake +cmake_minimum_required(VERSION 3.8) # was 3.5 +project(ed) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # needed by ament_clang_tidy + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) # required: enables std::filesystem (§3) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Werror=all) + add_compile_options(-Wextra -Werror=extra) +endif() +``` + +### 2.2 Dependency discovery + +Replace the single `find_package(catkin REQUIRED COMPONENTS ...)` with +`find_package(ament_cmake REQUIRED)` plus **one `find_package( REQUIRED)` +per dependency**. Delete the `catkin_package(...)` block entirely. + +```cmake +# ROS 1 +find_package(catkin REQUIRED COMPONENTS code_profiler geolib2 pluginlib rgbd + rgbd_msgs roscpp tf2_ros tue_config tue_serialization ...) +catkin_package(INCLUDE_DIRS include LIBRARIES ... CATKIN_DEPENDS ... DEPENDS ...) + +# ROS 2 +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(pluginlib REQUIRED) +find_package(code_profiler REQUIRED) +find_package(geolib2 REQUIRED) +find_package(rgbd REQUIRED) +find_package(rgbd_interfaces REQUIRED) +find_package(ed_interfaces REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tue_config REQUIRED) +find_package(tue_serialization REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(rosconsole_bridge REQUIRED) # routes dependencies' console_bridge logs to ROS (§6) +# non-ROS deps stay as they were: +find_package(OpenCV REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(orocos_kdl REQUIRED) +find_package(SDFormat REQUIRED) +find_package(TinyXML2 REQUIRED) +# NOTE: no find_package(tue_filesystem) — removed (§3) +``` + +### 2.3 Includes and linking — go target-based + +Drop the global `include_directories(... ${catkin_INCLUDE_DIRS})`. For each +library/executable, set includes on the target and link explicit, namespaced +targets (no more `${catkin_LIBRARIES}` / `add_dependencies(... ${catkin_EXPORTED_TARGETS})`). + +```cmake +add_library(${PROJECT_NAME}_core SHARED ${HEADER_FILES} src/...) +target_include_directories(${PROJECT_NAME}_core PUBLIC + $ + $) +target_include_directories(${PROJECT_NAME}_core SYSTEM PRIVATE + 3rdparty/polypartition/include + 3rdparty/rapidjson/include + ${PCL_INCLUDE_DIRS} ${SDFormat_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME}_core + polypartition + geolib2::geolib2 + tue_config::tue_config + tue_serialization::tue_serialization + rgbd::rgbd + console_bridge::console_bridge + tinyxml2::tinyxml2 + ${PCL_LIBRARIES} ${SDFormat_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME}_core PUBLIC diagnostic_updater tf2_ros) +``` + +For **generated interfaces**, link the typesupport target, not a bare name: + +```cmake +target_link_libraries(${PROJECT_NAME}_server + ${PROJECT_NAME}_core ${PROJECT_NAME}_io + rclcpp::rclcpp + rosconsole_bridge::rosconsole_bridge # keep the bridge in the linked-together node (§6) + ${ed_interfaces_TARGETS}) # or ed_interfaces::ed_interfaces__rosidl_typesupport_cpp +``` + +> Check exactly which target name a dependency exports with +> `cmake --find-package` or by reading `rgbd/master` and `geolib2/master` +> `CMakeLists.txt`. `geolib2` links e.g. +> `geometry_msgs::geometry_msgs__rosidl_generator_cpp` and `tf2::tf2`. + +### 2.4 Install + export + +```cmake +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) +install(FILES plugins.xml DESTINATION share/${PROJECT_NAME}) + +install(TARGETS ${PROJECT_NAME}_core ${PROJECT_NAME}_io ${PROJECT_NAME}_server + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) + +# executables and plugin .so's go under lib/${PROJECT_NAME} +install(TARGETS ${PROJECT_NAME} configure ${PROJECT_NAME}_view_model ... + DESTINATION lib/${PROJECT_NAME}) + +# Python tools: was catkin_install_python(...) +install(PROGRAMS tools/entity-teleop tools/list_plugins + DESTINATION lib/${PROJECT_NAME}) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(geolib2 tue_config tue_serialization rgbd rclcpp + pluginlib tf2_ros diagnostic_updater rosconsole_bridge ed_interfaces OpenCV PCL) +``` + +End the file with `ament_package()` (must be the **last** call). + +### 2.5 Tests / linters + +```cmake +# ROS 1: if (CATKIN_ENABLE_TESTING) ... catkin_add_gtest(...) / catkin_lint +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_clang_format REQUIRED) + find_package(ament_cmake_clang_tidy REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_xmllint REQUIRED) + find_package(tue_lint_config REQUIRED) + + ament_add_gtest(${PROJECT_NAME}_test_wm test/test_wm.cpp) + target_link_libraries(${PROJECT_NAME}_test_wm ${PROJECT_NAME}_core ${OpenCV_LIBRARIES}) + + ament_clang_format(CONFIG_FILE ${tue_lint_config_DIR}/../config/.clang-format --clang-format-version=21) + ament_clang_tidy(CONFIG_FILE ${tue_lint_config_DIR}/../config/.clang-tidy --clang-tidy-version=21 + ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_BINARY_DIR}/compile_commands.json) + ament_lint_cmake(MAX_LINE_LENGTH 120 "--filter=...") # copy filter from geolib2/master + ament_xmllint(MAX_LINE_LENGTH 120) +endif() +``` + +--- + +## 3. Remove `tue_filesystem` → `std::filesystem` (deprecated dependency) + +`tue_filesystem` is obsolete. **Do not depend on it.** Replace every use with +C++17 `` following its own skill: +`tue_filesystem/doc/migrate_to_std_filesystem.md`. + +In `ed`, only `tue::filesystem::Path` is used (no `Crawler`). Affected files: +`src/io/filesystem/read.cpp`, `src/io/filesystem/write.cpp`, +`src/models/load_model.cpp`, `src/models/model_loader.cpp`, +`src/models/shape_loader.cpp`, `src/server.cpp`, `tools/configure.cpp`, +`tools/view_model.cpp`. + +Key substitutions (full table in that skill): + +| `tue::filesystem::Path` | `std::filesystem` | +| --- | --- | +| `#include ` | `#include ` | +| `tue::filesystem::Path` | `std::filesystem::path` | +| `p.exists()` | `std::filesystem::exists(p)` | +| `p.extension()` *(returns `std::string`)* | `p.extension().string()` | +| `p.filename()` *(returns `std::string`)* | `p.filename().string()` | +| `p.parentPath()` | `p.parent_path()` — **quirk:** returns empty, not `"."` | +| `Path(a) + "/" + b` | `std::filesystem::path(a) / b` | + +Then remove `find_package(tue_filesystem ...)`, the `tue_filesystem` link/export +entries, and the `tue_filesystem` line. Verify clean: +`grep -rn "tue/filesystem\|tue::filesystem\|tue_filesystem" .` + +--- + +## 4. C++ node API: roscpp → rclcpp + +(Model: `rgbd/master`.) Header include style for **all** ROS message packages +changes from `` to `` / `` +(snake_case), and types gain `::msg::` / `::srv::`: + +```cpp +// ROS 1 // ROS 2 +#include #include +ed_msgs::Query::Request req; ed_interfaces::srv::Query::Request req; +#include #include +#include #include +``` + +Core API mapping: + +| ROS 1 (roscpp) | ROS 2 (rclcpp) | +| --- | --- | +| `#include ` | `#include ` | +| `ros::init(argc,argv,"ed")` | `rclcpp::init(argc,argv)` | +| `ros::NodeHandle nh; nh_private("~")` | `auto node = rclcpp::Node::make_shared("ed");` | +| `nh.advertise(topic,q)` | `node->create_publisher(topic,q)` | +| `pub.publish(m)` | `pub->publish(m)` | +| `pub.getNumSubscribers()` | `pub->get_subscription_count()` | +| `nh.subscribe(topic,q,cb)` | `node->create_subscription(topic,q,cb)` | +| `nh.advertiseService(name,cb)` | `node->create_service(name,cb)` (cb takes `Request::SharedPtr`, `Response::SharedPtr`, returns `void`) | +| `nh.serviceClient(name)` | `node->create_client(name)` | +| `nh.getParam("rate",r)` / `nh.param(...)` | `node->declare_parameter("rate",30.0)` | +| `ros::ok()` | `rclcpp::ok()` | +| `ros::spinOnce()` / `ros::spin()` | `rclcpp::spin_some(node)` / `rclcpp::spin(node)` | +| `ros::CallbackQueue` | `rclcpp::CallbackGroup` + an executor (`SingleThreadedExecutor`) | +| `ros::package::getPath("x")` | `ament_index_cpp::get_package_share_directory("x")` | + +Time / rates (used in `src/ed.cpp`, `src/plugin_container.cpp`, +`plugins/*.cpp`): + +| ROS 1 | ROS 2 | +| --- | --- | +| `ros::Time::now()` | `node->now()` or `clock->now()` | +| `ros::Time` (in headers, no node) | `rclcpp::Time` | +| `ros::Duration(1)` | `rclcpp::Duration::from_seconds(1)` | +| `ros::Rate r(f); r.sleep()` | `rclcpp::Rate r(f); r.sleep()` | +| `stamp.fromSec(t)` / `stamp.toSec()` | `rclcpp::Time(int64_t(t*1e9))` / `rclcpp::Time(stamp).seconds()` | +| `ros::Time::init()` (tests) | not needed; create a `rclcpp::Clock` | + +`tf2_ros::Buffer` now needs a clock: `tf2_ros::Buffer buffer(node->get_clock())`. +For header-location differences across distros, guard with `__has_include` +(see `geolib2/master` for tf2/image_geometry/cv_bridge examples): + +```cpp +#if __has_include() +#include +#else +#include +#endif +``` + +--- + +## 5. Interface (message/service) dependencies + +`ed` only **consumes** interfaces — it does not define them (those moved to +`ed_interfaces`). So no `rosidl` work here; just: + +- depend on `ed_interfaces` / `rgbd_interfaces` / + `tue_serialization_interfaces` (§1, §2.2), +- update includes and type names to `::msg::` / `::srv::` (§4), +- link the typesupport target (§2.3). + +Note `ed_interfaces` already changed field types: `time` → +`builtin_interfaces/Time`, `duration` → `builtin_interfaces/Duration`, +`tue_serialization/Binary` → `tue_serialization_interfaces/Binary`. Any code +in `include/ed/helpers/msg_conversions.h` that builds these fields must use the +new types. + +--- + +## 6. Logging + +`ed` has its own logger (`include/ed/logging.h`, `src/logging.cpp`) — **keep +it**. The ROS-coupled pieces to change: + +- **Keep `src/rosconsole_bridge.cpp`** (`#include ` + + `REGISTER_ROSCONSOLE_BRIDGE;` — both unchanged in ROS 2) and keep its source + entry in `CMakeLists.txt`. `rosconsole_bridge` is migrated to ROS 2 and stays + a dependency. `ed`'s recursive dependencies (geolib2, tue_config, …) log via + `console_bridge`, whose default handler only goes to stderr; the bridge's + registration installs a handler that forwards those messages into ROS 2 + logging (rosout). Without it those logs would not surface through ROS. Library + packages (geolib2 etc.) deliberately do **not** register the bridge — the end + application (here `ed`) owns that, so register it exactly once in the node. +- Replace the few `ROS_*` macros (`ROS_ERROR_STREAM`, `ROS_WARN_NAMED` in + `plugins/robot_plugin.cpp`, `plugins/sync_plugin.cpp`, + `src/models/load_model.cpp`) with either the existing `ed::log::*` API or + `RCLCPP_*(get_logger(), ...)` where a node/logger is in scope. +- Library code with no node uses `console_bridge`: + `CONSOLE_BRIDGE_logError("...: %s", e.c_str())`. + +--- + +## 7. Plugins (pluginlib) and components + +`ed`'s **own** plugin system (`ed::Plugin` base, `ED_REGISTER_PLUGIN`, +`pluginlib::ClassLoader` in `src/plugin_container.cpp`, +`plugins.xml`) stays on **pluginlib** — pluginlib is fully supported in ROS 2. +Changes: + +- Include path: `#include ` → + `#include `. `PLUGINLIB_EXPORT_CLASS` + (wrapped by `ED_REGISTER_PLUGIN`) is unchanged. +- `plugins.xml` content is unchanged (same `` + scheme). Export it the ROS 2 way instead of via the `` tag: + add to `CMakeLists.txt` + `pluginlib_export_plugin_description_file(ed plugins.xml)` and keep the + `install(FILES plugins.xml DESTINATION share/${PROJECT_NAME})`. +- Build plugin libraries as `SHARED`. + +Distinguish this from **nodelets**: if any tue package used `nodelet` +(`rgbd` did), that becomes an `rclcpp_components` component +(`RCLCPP_COMPONENTS_REGISTER_NODE` + `rclcpp_components_register_nodes(...)`). +`ed`'s plugins are *not* nodelets, so they stay pluginlib. + +--- + +## 8. Launch and Python tools + +- `*.launch` (XML) → `*.launch.py` (Python `LaunchDescription` / `Node`). + `type=` becomes `executable=`; `` becomes `remappings=[(from,to)]`. +- Python tools (`tools/entity-teleop`, `tools/list_plugins`): port any + `rospy`/`rospkg`/`roslib` use to `rclpy` / `ament_index_python`; install via + `install(PROGRAMS ... DESTINATION lib/${PROJECT_NAME})`. + +--- + +## 9. Boost → std (do alongside, low risk) + +ROS 2 / C++17 lets you drop Boost for the standard library. `include/ed/types.h` +already typedefs the smart pointers, so most call sites are insulated. Replace: +`boost::shared_ptr`→`std::shared_ptr`, `boost::make_shared`→`std::make_shared`, +`boost::mutex`/`scoped_lock`/`unique_lock`/`lock_guard`→`std::` equivalents, +`boost::thread`→`std::thread`, `boost::this_thread`→`std::this_thread`. This is +optional for a first build but removes a dependency; keep it a separate commit. + +--- + +## 10. CI + +Update `.github/workflows/main.yml` to the ROS 2 matrix used by the reference +packages. **Which template depends on how many packages the repo contains** — +count the `package.xml` files (`find . -name package.xml -not -path '*/.git/*'`): + +| Repo layout | Reference | Template | +| --- | --- | --- | +| **Single package** (one `package.xml`) | `geolib2`, `tue_config`, `upower_ros`, `ed_msgs` | §10.1 | +| **Multi-package** (≥2 `package.xml`) | `code_profiler`, `rgbd`, `tue_serialization` | §10.2 | + +`ed` ships a single `package.xml`, so it uses the **single-package** form +(§10.1). Migrate a multi-package repo (e.g. a future `rgbd`-style repo) with +§10.2. + +### 10.1 Single-package repo + +One `tue-ci` job over the `ros_distro` matrix (copy from `geolib2/master`): + +```yaml +name: CI + +on: [push, pull_request, workflow_dispatch] + +permissions: + contents: read + +jobs: + tue-ci: + name: TUe CI - ${{ github.event_name }} (${{ matrix.ros_distro }}) + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + ros_distro: [humble, jazzy, rolling-u24] + steps: + - name: TUe CI + uses: tue-robotics/tue-env/ci/main@master + with: + image: ghcr.io/tue-robotics/tue-env-ros-${{ matrix.ros_distro }} + package: ${{ github.event.repository.name }} +``` + +> **Package name ≠ repo name?** If the package was renamed during migration +> (e.g. the `ed_msgs` repo now ships `ed_interfaces`), `${{ github.event.repository.name }}` +> no longer matches. Hardcode the actual package name instead — `ed_msgs` +> uses `package: ed_interfaces`. For `ed` the names match, so keep the +> `repository.name` expression. + +### 10.2 Multi-package repo + +A first `matrix` job determines which packages changed (so CI only builds the +affected ones), feeding a 2-D `ros_distro` × `package` matrix in `tue-ci` (copy +from `rgbd/master` or `tue_serialization/master`): + +```yaml +name: CI + +on: [push, pull_request, workflow_dispatch] + +permissions: + contents: read + +jobs: + matrix: + name: Determine modified packages + runs-on: ubuntu-latest + outputs: + packages: ${{ steps.modified-packages.outputs.packages }} + steps: + - uses: actions/checkout@v7 + with: + fetch-depth: 300 # deep enough for the commit-range diff + - name: Commit Range + id: commit-range + uses: tue-robotics/tue-env/ci/commit-range@master + - name: Modified packages + id: modified-packages + uses: tue-robotics/tue-env/ci/modified-packages@master + with: + commit-range: ${{ steps.commit-range.outputs.commit-range }} + tue-ci: + name: TUe CI - ${{ matrix.package }} (${{ matrix.ros_distro }}) + runs-on: ubuntu-latest + needs: matrix + strategy: + fail-fast: false + matrix: + ros_distro: [humble, jazzy, rolling-u24] + package: ${{ fromJson(needs.matrix.outputs.packages) }} + steps: + - name: TUe CI + uses: tue-robotics/tue-env/ci/main@master + with: + image: ghcr.io/tue-robotics/tue-env-ros-${{ matrix.ros_distro }} + package: ${{ matrix.package }} +``` + +> The `package` matrix is populated dynamically, so it needs **no edits** when +> packages are added or renamed inside the repo. Do **not** copy the +> `default-branch:` input some in-flight migration branches carry (e.g. +> `rgbd`'s `copilot/...`) — that is a temporary artifact, not part of the +> template. + +--- + +## 11. Validation + +```bash +colcon build --packages-up-to ed +colcon test --packages-select ed && colcon test-result --verbose +# no leftover ROS 1 / tue_filesystem references: +grep -rn "ros/ros.h\|ros::\|ROS_INFO\|ROS_WARN\|ROS_ERROR\|catkin\|tue_filesystem\|ed_msgs\|rgbd_msgs" \ + src include plugins tools test CMakeLists.txt package.xml +``` + +Acceptable remaining hits: comments/changelog you intentionally wrote. + +--- + +## 12. Pitfalls checklist (review before submitting) + +- [ ] `package.xml`: `ament_cmake`, no `catkin`/`message_generation`/`roslib`/ + `tue_filesystem`; `rosconsole_bridge` **kept**; `*_msgs`→`*_interfaces`; + `ament_cmake` present. +- [ ] `CMakeLists.txt`: one `find_package` per dep; no `catkin_package`; + target-based includes/links; `ament_export_targets` + + `ament_export_dependencies`; `ament_package()` last. +- [ ] All ROS message includes converted to `` and types to + `::msg::`/`::srv::`; interfaces linked via the typesupport target. +- [ ] roscpp→rclcpp complete; service callbacks return `void` and take + `SharedPtr` args; `ros::Time`/`Duration`/`Rate`→`rclcpp::`. +- [ ] `tf2_ros::Buffer` constructed with a clock. +- [ ] `tue_filesystem` fully removed in favour of `std::filesystem`; C++17 set. +- [ ] `rosconsole_bridge` kept (dep + `REGISTER_ROSCONSOLE_BRIDGE` registration) + so dependencies' `console_bridge` logs reach ROS; `ROS_*` macros replaced. +- [ ] pluginlib: `class_list_macros.hpp` include; plugin libs `SHARED`; + `pluginlib_export_plugin_description_file(...)` added. +- [ ] Launch files ported to `.launch.py`; Python tools on rclpy. +- [ ] CI matrix updated; `colcon build`/`test` green on targeted distros. + +--- + +## 13. Commit / PR + +Stage the work as logical commits, mirroring the reference PRs (e.g. +ed_msgs#10 "Migrate to ROS 2"): + +1. `Migrate from tue_filesystem to std::filesystem` (§3, isolated). +2. `Migrate ed to ROS 2 (ament_cmake + rclcpp)` (§1–§8). +3. `Replace Boost with std` (§9, optional). +4. `CI: ROS 2 distro matrix` (§10). + + diff --git a/examples/custom_properties/counter_info.h b/examples/custom_properties/counter_info.h index e80b3e5b..206fdca7 100644 --- a/examples/custom_properties/counter_info.h +++ b/examples/custom_properties/counter_info.h @@ -7,24 +7,26 @@ class CounterInfo : public ed::PropertyInfo { public: - - void serialize(const ed::Variant& v, ed::io::Writer& w) const + void serialize(const ed::Variant& v, ed::io::Writer& w) const override { - int counter = v.getValue(); + int const counter = v.getValue(); w.writeValue("value", counter); } - bool deserialize(ed::io::Reader& r, ed::Variant& v) const + bool deserialize(ed::io::Reader& r, ed::Variant& v) const override { - int counter; + int counter = 0; r.readValue("value", counter); v.setValue(counter); return true; } - bool serializable() const { return true; } - + [[nodiscard]] + bool serializable() const override + { + return true; + } }; #endif diff --git a/examples/custom_properties/custom_properties_plugin.cpp b/examples/custom_properties/custom_properties_plugin.cpp index 26de0ac8..7628c90c 100644 --- a/examples/custom_properties/custom_properties_plugin.cpp +++ b/examples/custom_properties/custom_properties_plugin.cpp @@ -1,24 +1,26 @@ #include "custom_properties_plugin.h" -#include -#include #include +#include +#include +#include +#include +#include // Property info -#include "pose_info.h" #include "counter_info.h" +#include "ed/init_data.h" +#include "ed/plugin.h" +#include "ed/types.h" +#include "pose_info.h" // ---------------------------------------------------------------------------------------------------- -CustomProperties::CustomProperties() -{ -} +CustomProperties::CustomProperties() = default; // ---------------------------------------------------------------------------------------------------- -CustomProperties::~CustomProperties() -{ -} +CustomProperties::~CustomProperties() = default; // ---------------------------------------------------------------------------------------------------- @@ -37,7 +39,7 @@ void CustomProperties::initialize(ed::InitData& init) void CustomProperties::process(const ed::WorldModel& world, ed::UpdateRequest& req) { // Find the entity with id 'test-entity' - ed::EntityConstPtr e = world.getEntity("test-entity"); + ed::EntityConstPtr const e = world.getEntity("test-entity"); if (!e) { @@ -58,7 +60,7 @@ void CustomProperties::process(const ed::WorldModel& world, ed::UpdateRequest& r // property was not found OR the type did not match. Therefore, always check if the pointer is not null! if (pose) { - std::cout << "Entity " << e->id() << " has pose " << *pose << std::endl; + std::cout << "Entity " << e->id() << " has pose " << *pose << '\n'; // Add a little offset to the current pose ... geo::Pose3D new_pose = *pose; @@ -73,17 +75,17 @@ void CustomProperties::process(const ed::WorldModel& world, ed::UpdateRequest& r if (!counter) { // The first time the plugin tries to access the counter property, the property is not yet there. - std::cout << "Counter property does not yet exist. Will be created and initialized to 0" << std::endl; + std::cout << "Counter property does not yet exist. Will be created and initialized to 0" << '\n'; req.setProperty(e->id(), k_counter_, 0); } else { // The second time the counter property will be set, so we can access it and update - std::cout << "Entity " << e->id() << " has counter " << *counter << std::endl; + std::cout << "Entity " << e->id() << " has counter " << *counter << '\n'; req.setProperty(e->id(), k_counter_, *counter + 1); } - std::cout << std::endl; + std::cout << '\n'; } } diff --git a/examples/custom_properties/custom_properties_plugin.h b/examples/custom_properties/custom_properties_plugin.h index 25c7fed4..9ecb8c24 100644 --- a/examples/custom_properties/custom_properties_plugin.h +++ b/examples/custom_properties/custom_properties_plugin.h @@ -9,17 +9,15 @@ class CustomProperties : public ed::Plugin { public: - CustomProperties(); - virtual ~CustomProperties(); + ~CustomProperties() override; - void initialize(ed::InitData& init); + void initialize(ed::InitData& init) override; - void process(const ed::WorldModel& world, ed::UpdateRequest& req); + void process(const ed::WorldModel& world, ed::UpdateRequest& req) override; private: - // Create a key for each entity property you want to acces. This key // will be used to access that property. Note that you should specify // the type of the property using templates @@ -29,7 +27,6 @@ class CustomProperties : public ed::Plugin // 'Counter' property key ed::PropertyKey k_counter_; - }; #endif diff --git a/examples/custom_properties/pose_info.h b/examples/custom_properties/pose_info.h index 198eb3e8..a1de8617 100644 --- a/examples/custom_properties/pose_info.h +++ b/examples/custom_properties/pose_info.h @@ -7,8 +7,7 @@ class PoseInfo : public ed::PropertyInfo { public: - - void serialize(const ed::Variant& v, ed::io::Writer& w) const + void serialize(const ed::Variant& v, ed::io::Writer& w) const override { const geo::Pose3D& p = v.getValue(); @@ -31,7 +30,7 @@ class PoseInfo : public ed::PropertyInfo w.endGroup(); } - bool deserialize(ed::io::Reader& r, ed::Variant& v) const + bool deserialize(ed::io::Reader& r, ed::Variant& v) const override { geo::Pose3D p = geo::Pose3D::identity(); @@ -61,8 +60,11 @@ class PoseInfo : public ed::PropertyInfo return true; } - bool serializable() const { return true; } - + [[nodiscard]] + bool serializable() const override + { + return true; + } }; #endif diff --git a/examples/hello_world/hello_world_plugin.cpp b/examples/hello_world/hello_world_plugin.cpp index 85965736..fb00ec98 100644 --- a/examples/hello_world/hello_world_plugin.cpp +++ b/examples/hello_world/hello_world_plugin.cpp @@ -1,4 +1,7 @@ #include "hello_world_plugin.h" +#include "ed/init_data.h" +#include "ed/plugin.h" +#include "ed/types.h" #include @@ -6,15 +9,11 @@ // ---------------------------------------------------------------------------------------------------- -HelloWorld::HelloWorld() -{ -} +HelloWorld::HelloWorld() = default; // ---------------------------------------------------------------------------------------------------- -HelloWorld::~HelloWorld() -{ -} +HelloWorld::~HelloWorld() = default; // ---------------------------------------------------------------------------------------------------- @@ -27,7 +26,7 @@ void HelloWorld::initialize(ed::InitData& init) void HelloWorld::process(const ed::WorldModel& /*world*/, ed::UpdateRequest& /*req*/) { - std::cout << text_ << std::endl; + std::cout << text_ << '\n'; ed::log::info(text_); ed::log::warning(text_); diff --git a/examples/hello_world/hello_world_plugin.h b/examples/hello_world/hello_world_plugin.h index bc92ed78..4a0c61b3 100644 --- a/examples/hello_world/hello_world_plugin.h +++ b/examples/hello_world/hello_world_plugin.h @@ -7,19 +7,16 @@ class HelloWorld : public ed::Plugin { public: - HelloWorld(); - virtual ~HelloWorld(); + ~HelloWorld() override; - void initialize(ed::InitData& init); + void initialize(ed::InitData& init) override; - void process(const ed::WorldModel& world, ed::UpdateRequest& req); + void process(const ed::WorldModel& world, ed::UpdateRequest& req) override; private: - std::string text_; - }; #endif diff --git a/include/ed/convex_hull.h b/include/ed/convex_hull.h index 708c2d70..be6ec145 100644 --- a/include/ed/convex_hull.h +++ b/include/ed/convex_hull.h @@ -13,18 +13,25 @@ struct ConvexHull std::vector points; std::vector edges; std::vector normals; - float z_min, z_max; - float area; // is calculated based on points - bool complete; - - ConvexHull() : area(0), complete(false) {} - - double height() const { return z_max - z_min; } - - double volume() const { return height() * area; } - + float z_min{}, z_max{}; + float area{0}; // is calculated based on points + bool complete{false}; + + ConvexHull() = default; + + [[nodiscard]] + double height() const + { + return z_max - z_min; + } + + [[nodiscard]] + double volume() const + { + return height() * area; + } }; -} +} // namespace ed #endif diff --git a/include/ed/convex_hull_2d.h b/include/ed/convex_hull_2d.h index 4766c58d..4cd78c99 100644 --- a/include/ed/convex_hull_2d.h +++ b/include/ed/convex_hull_2d.h @@ -1,8 +1,8 @@ #ifndef ED_CONVEX_HULL_2D_H_ #define ED_CONVEX_HULL_2D_H_ -#include #include +#include #include @@ -13,20 +13,25 @@ namespace ed { -typedef std::vector< std::vector > IndexMap; +using IndexMap = std::vector>; -struct ConvexHull2D { +struct ConvexHull2D +{ ConvexHull2D() : center_point(geo::Vector3(0, 0, 0)) {} pcl::PointCloud chull; // Convex hull point w.r.t. center - double min_z, max_z; // min and max z of convex hull + double min_z{}, max_z{}; // min and max z of convex hull geo::Vector3 center_point; // Center of the convex hull + [[nodiscard]] double area() const; + [[nodiscard]] double height() const; + [[nodiscard]] double volume() const; }; -struct ConvexHull2DWithIndices { +struct ConvexHull2DWithIndices +{ std::vector indices; ConvexHull2D convex_hull_2d; }; diff --git a/include/ed/convex_hull_calc.h b/include/ed/convex_hull_calc.h index a2eeaefb..9db69d36 100644 --- a/include/ed/convex_hull_calc.h +++ b/include/ed/convex_hull_calc.h @@ -5,10 +5,7 @@ #include -namespace ed -{ - -namespace convex_hull +namespace ed::convex_hull { void create(const std::vector& points, float z_min, float z_max, ConvexHull& chull, geo::Pose3D& pose); @@ -17,14 +14,15 @@ void createAbsolute(const std::vector& points, float z_min, float z_ void calculateEdgesAndNormals(ConvexHull& chull); -bool collide(const ConvexHull& c1, const geo::Vector3& pos1, - const ConvexHull& c2, const geo::Vector3& pos2, - float xy_padding = 0, float z_padding = 0); +bool collide(const ConvexHull& c1, + const geo::Vector3& pos1, + const ConvexHull& c2, + const geo::Vector3& pos2, + float xy_padding = 0, + float z_padding = 0); void calculateArea(ConvexHull& c); -} - -} +} // namespace ed::convex_hull #endif diff --git a/include/ed/entity.h b/include/ed/entity.h index c31ce279..bbb86288 100644 --- a/include/ed/entity.h +++ b/include/ed/entity.h @@ -1,15 +1,15 @@ #ifndef ED_ENTITY_H_ #define ED_ENTITY_H_ -#include "ed/types.h" -#include "ed/convex_hull_2d.h" #include "ed/convex_hull.h" +#include "ed/convex_hull_2d.h" +#include "ed/types.h" #include "ed/uuid.h" #include +#include #include -#include #include "ed/property.h" #include "ed/property_key.h" @@ -31,50 +31,74 @@ class Entity { public: - Entity(const UUID& id = generateID(), const TYPE& type = "", const unsigned int& measurement_buffer_size = 5); + Entity(UUID id = generateID(), TYPE type = "", const unsigned int& measurement_buffer_size = 5); ~Entity(); static UUID generateID(); - inline const UUID& id() const { return id_; } + const UUID& id() const { return id_; } - inline const TYPE& type() const { return type_; } - inline void setType(const TYPE& type) { type_ = type; types_.insert(type); } + const TYPE& type() const { return type_; } + void setType(const TYPE& type) + { + type_ = type; + types_.insert(type); + } - inline const std::set& types() const { return types_; } - inline void addType(const TYPE& type) { types_.insert(type); } - inline void removeType(const TYPE& type) { types_.erase(type); } - inline bool hasType(const TYPE& type) const { return types_.find(type) != types_.end(); } + const std::set& types() const { return types_; } + void addType(const TYPE& type) { types_.insert(type); } + void removeType(const TYPE& type) { types_.erase(type); } + bool hasType(const TYPE& type) const { return types_.find(type) != types_.end(); } void measurements(std::vector& measurements, double min_timestamp = 0) const; void measurements(std::vector& measurements, unsigned int num) const; MeasurementConstPtr lastMeasurement() const; - inline unsigned int measurementSeq() const { return measurements_seq_; } - inline MeasurementConstPtr bestMeasurement() const { return best_measurement_; } + unsigned int measurementSeq() const { return measurements_seq_; } + MeasurementConstPtr bestMeasurement() const { return best_measurement_; } - void addMeasurement(MeasurementConstPtr measurement); + void addMeasurement(const MeasurementConstPtr& measurement); [[deprecated("Use visual() or collision() instead.")]] - inline geo::ShapeConstPtr shape() const { return visual(); } - inline geo::ShapeConstPtr visual() const { return visual_; } - inline geo::ShapeConstPtr collision() const { return collision_; } + geo::ShapeConstPtr shape() const + { + return visual(); + } + geo::ShapeConstPtr visual() const { return visual_; } + geo::ShapeConstPtr collision() const { return collision_; } [[deprecated("Use setVisual() or setCollision() instead.")]] - inline void setShape(const geo::ShapeConstPtr& shape) { setVisual(shape); } + void setShape(const geo::ShapeConstPtr& shape) + { + setVisual(shape); + } void setVisual(const geo::ShapeConstPtr& visual); void setCollision(const geo::ShapeConstPtr& collision); - inline const std::map& volumes() const { return volumes_; } - inline void addVolume(const std::string& volume_name, const geo::ShapeConstPtr& volume_shape) { volumes_[volume_name] = volume_shape; ++volumes_revision_; } - inline void removeVolume(const std::string& volume_name) { volumes_.erase(volume_name); ++volumes_revision_; } + const std::map& volumes() const { return volumes_; } + void addVolume(const std::string& volume_name, const geo::ShapeConstPtr& volume_shape) + { + volumes_[volume_name] = volume_shape; + ++volumes_revision_; + } + void removeVolume(const std::string& volume_name) + { + volumes_.erase(volume_name); + ++volumes_revision_; + } [[deprecated("Use visualRevision(), collisionRevision() or volumesRevision() instead.")]] - inline unsigned long shapeRevision() const{ return visualRevision(); } - inline unsigned long visualRevision() const{ return visual_ ? visual_revision_ : 0; } - inline unsigned long collisionRevision() const{ return collision_ ? collision_revision_ : 0; } - inline unsigned long volumesRevision() const{ return !volumes_.empty() ? volumes_revision_ : 0; } + unsigned long shapeRevision() const + { + return visualRevision(); + } + unsigned long visualRevision() const { return visual_ ? visual_revision_ : 0; } + unsigned long collisionRevision() const { return collision_ ? collision_revision_ : 0; } + unsigned long volumesRevision() const { return !volumes_.empty() ? volumes_revision_ : 0; } - inline const ConvexHull& convexHull() const { return convex_hull_new_; } + const ConvexHull& convexHull() const { return convex_hull_new_; } - void setConvexHull(const ConvexHull& convex_hull, const geo::Pose3D& pose, double time, const std::string& source = "") + void setConvexHull(const ConvexHull& convex_hull, + const geo::Pose3D& pose, + double time, + const std::string& source = "") { if (convex_hull.points.empty()) { @@ -92,16 +116,16 @@ class Entity updateConvexHull(); } - inline const std::map& convexHullMap() const { return convex_hull_map_; } + const std::map& convexHullMap() const { return convex_hull_map_; } - inline const geo::Pose3D& pose() const + const geo::Pose3D& pose() const { if (!has_pose_) - log::warning() << "Someone's accessing an entity's pose while it doesnt have one." << std::endl; + log::warning() << "Someone's accessing an entity's pose while it doesnt have one." << '\n'; return pose_; } - inline void setPose(const geo::Pose3D& pose) + void setPose(const geo::Pose3D& pose) { pose_ = pose; if (visual_) @@ -110,45 +134,44 @@ class Entity has_pose_ = true; } - inline void removePose() { has_pose_ = false; } + void removePose() { has_pose_ = false; } - inline bool has_pose() const { return has_pose_; } + bool has_pose() const { return has_pose_; } - inline const tue::config::DataConstPointer& data() const { return config_; } - inline void setData(const tue::config::DataConstPointer& data) { config_ = data; } + const tue::config::DataConstPointer& data() const { return config_; } + void setData(const tue::config::DataConstPointer& data) { config_ = data; } -// inline double creationTime() const { return creation_time_; } + // inline double creationTime() const { return creation_time_; } - inline void setRelationTo(Idx child_idx, Idx r_idx) { relations_to_[child_idx] = r_idx; } + void setRelationTo(Idx child_idx, Idx r_idx) { relations_to_[child_idx] = r_idx; } - inline void setRelationFrom(Idx parent_idx, Idx r_idx) { relations_from_[parent_idx] = r_idx; } + void setRelationFrom(Idx parent_idx, Idx r_idx) { relations_from_[parent_idx] = r_idx; } - inline Idx relationTo(Idx child_idx) const + Idx relationTo(Idx child_idx) const { - std::map::const_iterator it = relations_to_.find(child_idx); + auto const it = relations_to_.find(child_idx); if (it == relations_to_.end()) return INVALID_IDX; return it->second; } - inline Idx relationFrom(Idx parent_idx) const + Idx relationFrom(Idx parent_idx) const { - std::map::const_iterator it = relations_from_.find(parent_idx); + auto const it = relations_from_.find(parent_idx); if (it == relations_from_.end()) return INVALID_IDX; return it->second; } - inline const std::map& relationsFrom() const { return relations_from_; } + const std::map& relationsFrom() const { return relations_from_; } - inline const std::map& relationsTo() const { return relations_to_; } + const std::map& relationsTo() const { return relations_to_; } - template - const T* property(const PropertyKey& key) const + template const T* property(const PropertyKey& key) const { - std::map::const_iterator it = properties_.find(key.idx); + auto const it = properties_.find(key.idx); if (it == properties_.end()) - return 0; + return nullptr; const Property& p = it->second; @@ -158,35 +181,35 @@ class Entity } catch (std::bad_cast& e) { - return 0; + return nullptr; } } -// template -// void setProperty(const PropertyKey& key, const T& t) -// { -// if (!key.valid()) -// return; - -// std::map::iterator it = properties_.find(key.idx); -// if (it == properties_.end()) -// { -// Property& p = properties_[key.idx]; -// p.entry = key.entry; -// p.revision = 0; -// p.value.setValue(t); -// } -// else -// { -// Property& p = it->second; -// p.value.setValue(t); -// ++(p.revision); -// } -// } + // template + // void setProperty(const PropertyKey& key, const T& t) + // { + // if (!key.valid()) + // return; + + // std::map::iterator it = properties_.find(key.idx); + // if (it == properties_.end()) + // { + // Property& p = properties_[key.idx]; + // p.entry = key.entry; + // p.revision = 0; + // p.value.setValue(t); + // } + // else + // { + // Property& p = it->second; + // p.value.setValue(t); + // ++(p.revision); + // } + // } void setProperty(Idx idx, const Property& p) { - std::map::iterator it = properties_.find(idx); + auto const it = properties_.find(idx); if (it == properties_.end()) { Property& p_new = properties_[idx]; @@ -201,64 +224,62 @@ class Entity p_new.revision = p.revision; } - if (revision_ < p.revision) - revision_ = p.revision; + revision_ = std::max(revision_, p.revision); } const std::map& properties() const { return properties_; } - inline unsigned long revision() const { return revision_; } + unsigned long revision() const { return revision_; } - inline void setRevision(unsigned long revision) { revision_ = revision; } + void setRevision(unsigned long revision) { revision_ = revision; } - inline void setExistenceProbability(double prob) { existence_prob_ = prob; } + void setExistenceProbability(double prob) { existence_prob_ = prob; } - inline double existenceProbability() const { return existence_prob_; } + double existenceProbability() const { return existence_prob_; } - inline void setLastUpdateTimestamp(double t) { last_update_timestamp_ = t; } + void setLastUpdateTimestamp(double t) { last_update_timestamp_ = t; } - inline double lastUpdateTimestamp() const { return last_update_timestamp_; } + double lastUpdateTimestamp() const { return last_update_timestamp_; } - inline void setFlag(const std::string& flag) { flags_.insert(flag); } + void setFlag(const std::string& flag) { flags_.insert(flag); } - inline void removeFlag(const std::string& flag) { flags_.erase(flag); } + void removeFlag(const std::string& flag) { flags_.erase(flag); } - inline bool hasFlag(const std::string& flag) const { return flags_.find(flag) != flags_.end(); } + bool hasFlag(const std::string& flag) const { return flags_.find(flag) != flags_.end(); } - inline const std::set& flags() const { return flags_; } + const std::set& flags() const { return flags_; } private: - UUID id_; - unsigned long revision_; + unsigned long revision_{0}; TYPE type_; std::set types_; - double existence_prob_; + double existence_prob_{1.0}; - double last_update_timestamp_; + double last_update_timestamp_{0}; boost::circular_buffer measurements_; MeasurementConstPtr best_measurement_; - unsigned int measurements_seq_; + unsigned int measurements_seq_{0}; geo::ShapeConstPtr visual_; geo::ShapeConstPtr collision_; std::map volumes_; - unsigned long visual_revision_; - unsigned long collision_revision_; - unsigned long volumes_revision_; + unsigned long visual_revision_{0}; + unsigned long collision_revision_{0}; + unsigned long volumes_revision_{0}; std::map convex_hull_map_; ConvexHull convex_hull_new_; - bool has_pose_; + bool has_pose_{false}; geo::Pose3D pose_; -// double creation_time_; + // double creation_time_; tue::config::DataConstPointer config_; @@ -273,9 +294,8 @@ class Entity void updateConvexHullFromVisual(); std::set flags_; - }; -} +} // namespace ed #endif diff --git a/include/ed/error_context.h b/include/ed/error_context.h index f0fe4966..c915e276 100644 --- a/include/ed/error_context.h +++ b/include/ed/error_context.h @@ -8,22 +8,20 @@ namespace ed struct ErrorContextData { - std::vector > stack; + std::vector> stack; }; class ErrorContext { public: - - ErrorContext(const char* msg, const char* value = 0); + ErrorContext(const char* msg, const char* value = nullptr); ~ErrorContext(); - void change(const char* msg, const char* value = 0); + static void change(const char* msg, const char* value = nullptr); static ErrorContextData* data(); - }; } // end namespace ed diff --git a/include/ed/event_clock.h b/include/ed/event_clock.h index 7010c84b..73bdf203 100644 --- a/include/ed/event_clock.h +++ b/include/ed/event_clock.h @@ -9,17 +9,16 @@ namespace ed class EventClock { public: - EventClock() : cycle_duration_(0), t_last_trigger_(0) {} EventClock(double freq) : cycle_duration_(1.0 / freq), t_last_trigger_(0) {} bool triggers() { - struct timeval now; - gettimeofday(&now, NULL); + struct timeval now{}; + gettimeofday(&now, nullptr); - double t_secs = now.tv_sec + now.tv_usec / 1e6; + double const t_secs = now.tv_sec + (now.tv_usec / 1e6); if ((t_secs - t_last_trigger_) > cycle_duration_) { t_last_trigger_ = t_secs; @@ -33,6 +32,6 @@ class EventClock double t_last_trigger_; }; -} +} // namespace ed #endif diff --git a/include/ed/helpers/msg_conversions.h b/include/ed/helpers/msg_conversions.h index b2ccd5c2..69597ae6 100644 --- a/include/ed/helpers/msg_conversions.h +++ b/include/ed/helpers/msg_conversions.h @@ -1,39 +1,44 @@ #ifndef ED_HELPERS_MSG_CONVERSIONS_H_ #define ED_HELPERS_MSG_CONVERSIONS_H_ -#include "ed_msgs/EntityInfo.h" #include "ed/entity.h" +#include "ed_interfaces/msg/entity_info.hpp" +#include "ed_interfaces/msg/sub_volume.hpp" +#include "ed_interfaces/msg/volume.hpp" -#include "geolib/Shape.h" #include "geolib/Box.h" #include "geolib/CompositeShape.h" #include "geolib/datatypes.h" #include "geolib/ros/msg_conversions.h" +#include "geolib/Shape.h" #include "tue/config/yaml_emitter.h" -#include +#include + +#include -namespace ed { +namespace ed +{ // ------------------------------ TO ROS ------------------------------ /** - * @brief converting geo::ShapeConstPtr to ed_msgs::SubVolume message + * @brief converting geo::ShapeConstPtr to ed_interfaces::msg::SubVolume message * @param shape geo::ShapeConstPtr as input - * @param msg filled ed_msgs::SubVolume message as output + * @param msg filled ed_interfaces::msg::SubVolume message as output */ -void convert(const geo::ShapeConstPtr shape, ed_msgs::SubVolume& sub_Volume) +void convert(const geo::ShapeConstPtr shape, ed_interfaces::msg::SubVolume& sub_Volume) { - geo::Vector3 min = shape->getBoundingBox().getMin(); - geo::Vector3 max = shape->getBoundingBox().getMax(); + geo::Vector3 const min = shape->getBoundingBox().getMin(); + geo::Vector3 const max = shape->getBoundingBox().getMax(); - geo::Vector3 pos = (min + max)/2; - geo::Vector3 size = max - min; + geo::Vector3 const pos = (min + max) / 2; + geo::Vector3 const size = max - min; geo::convert(pos, sub_Volume.center_point.point); - shape_msgs::SolidPrimitive solid; + shape_msgs::msg::SolidPrimitive const solid; sub_Volume.geometry.type = sub_Volume.geometry.BOX; sub_Volume.geometry.dimensions.resize(3, 0); sub_Volume.geometry.dimensions[solid.BOX_X] = size.x; @@ -46,12 +51,13 @@ void convert(const geo::ShapeConstPtr shape, ed_msgs::SubVolume& sub_Volume) * @param e ed::Entity as input * @param msg filled ed_msgs::EntityInfo message as output */ -void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) { +void convert(const ed::Entity& e, ed_interfaces::msg::EntityInfo& msg) +{ msg.id = e.id().str(); msg.type = e.type(); msg.types.resize(0); - for(std::set::const_iterator it = e.types().begin(); it != e.types().end(); ++it) + for (std::set::const_iterator it = e.types().begin(); it != e.types().end(); ++it) msg.types.push_back(*it); msg.existence_probability = e.existenceProbability(); @@ -64,7 +70,7 @@ void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) { msg.z_max = convex_hull.z_max; msg.convex_hull.resize(convex_hull.points.size()); - for(unsigned int i = 0; i < msg.convex_hull.size(); ++i) + for (unsigned int i = 0; i < msg.convex_hull.size(); ++i) { msg.convex_hull[i].x = convex_hull.points[i].x; msg.convex_hull[i].y = convex_hull.points[i].y; @@ -85,7 +91,7 @@ void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) { geo::convert(e.pose(), msg.pose); } - msg.last_update_time = ros::Time(e.lastUpdateTimestamp()); + msg.last_update_time = rclcpp::Time(static_cast(e.lastUpdateTimestamp() * 1e9)); if (!e.data().empty()) { @@ -102,30 +108,35 @@ void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) { if (!e.volumes().empty()) { - for (std::map::const_iterator it = e.volumes().begin(); it != e.volumes().end(); ++it) + for (std::map::const_iterator it = e.volumes().begin(); + it != e.volumes().end(); + ++it) { - ed_msgs::Volume volume; + ed_interfaces::msg::Volume volume; volume.name = it->first; - geo::CompositeShapeConstPtr composite = std::dynamic_pointer_cast(it->second); + geo::CompositeShapeConstPtr const composite = + std::dynamic_pointer_cast(it->second); if (composite) { - const std::vector >& shapes = composite->getShapes(); - for (std::vector >::const_iterator it2 = shapes.begin(); - it2 != shapes.end(); ++it2) + const std::vector>& shapes = composite->getShapes(); + for (std::vector>::const_iterator it2 = shapes.begin(); + it2 != shapes.end(); + ++it2) { - geo::ShapePtr shape_tr(new geo::Shape()); - shape_tr->setMesh(it2->first->getMesh().getTransformed(it2->second.inverse())); + geo::ShapePtr const shape_tr(new geo::Shape()); + geo::Transform inv = it2->second.inverse(); + shape_tr->setMesh(it2->first->getMesh().getTransformed(inv)); - ed_msgs::SubVolume sub_volume; - convert(shape_tr, sub_volume); + ed_interfaces::msg::SubVolume sub_volume; + convert(shape_tr, sub_volume); sub_volume.center_point.header.frame_id = e.id().str(); volume.subvolumes.push_back(sub_volume); } } else { - ed_msgs::SubVolume sub_volume; + ed_interfaces::msg::SubVolume sub_volume; convert(it->second, sub_volume); volume.subvolumes.push_back(sub_volume); } @@ -139,12 +150,12 @@ void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) { // Flags msg.flags.resize(0); - for(std::set::const_iterator it = e.flags().begin(); it != e.flags().end(); ++it) + for (std::set::const_iterator it = e.flags().begin(); it != e.flags().end(); ++it) msg.flags.push_back(*it); } // ------------------------------ FROM ROS ------------------------------ -} +} // namespace ed #endif diff --git a/include/ed/init_data.h b/include/ed/init_data.h index f1dd3d40..7dd3c13a 100644 --- a/include/ed/init_data.h +++ b/include/ed/init_data.h @@ -11,13 +11,12 @@ namespace ed struct InitData { - InitData(ed::PropertyKeyDB& properties_, tue::Configuration& config_) - : properties(properties_), config(config_) {} + InitData(ed::PropertyKeyDB& properties_, tue::Configuration& config_) : properties(properties_), config(config_) {} ed::PropertyKeyDB& properties; tue::Configuration& config; }; -} // end namespace +} // namespace ed #endif diff --git a/include/ed/io/binary_writer.h b/include/ed/io/binary_writer.h index 07fe68da..045cc37d 100644 --- a/include/ed/io/binary_writer.h +++ b/include/ed/io/binary_writer.h @@ -17,20 +17,13 @@ class BinaryWriter : Writer { public: - BinaryWriter(Data& data) : Writer(data) {} ~BinaryWriter() {} - void writeGroup(const std::string& name) - { - writeLabel("g" + name); - } + void writeGroup(const std::string& name) { writeLabel("g" + name); } - void endGroup() - { - write('e'); - } + void endGroup() { write('e'); } void writeValue(const std::string& key, float f) { @@ -66,23 +59,17 @@ class BinaryWriter : Writer // ... } - void writeArray(const std::string& key) - { - writeLabel("a" + key); - } + void writeArray(const std::string& key) { writeLabel("a" + key); } void addArrayItem() {} void endArrayItem() {} - void endArray() - { - write('e'); - } + void endArray() { write('e'); } void finish() { // Add all labels - for(std::vector::const_iterator it = labels_.begin(); it != labels_.end(); ++it) + for (std::vector::const_iterator it = labels_.begin(); it != labels_.end(); ++it) { const std::string& s = *it; data_.insert(data_.end(), &s[0], &s[s.size() + 1]); @@ -92,7 +79,6 @@ class BinaryWriter : Writer } private: - std::vector labels_; std::map label_to_index_; @@ -121,17 +107,15 @@ class BinaryWriter : Writer } } - template - inline void write(const T& d) + template inline void write(const T& d) { temp_data_.insert(temp_data_.end(), (char*)&d, (char*)&d + sizeof(T)); } std::vector temp_data_; - }; -} +} // namespace io } // end namespace era diff --git a/include/ed/io/data.h b/include/ed/io/data.h index 1a1784fc..fe00d2e5 100644 --- a/include/ed/io/data.h +++ b/include/ed/io/data.h @@ -3,14 +3,11 @@ #include "ed/io/variant.h" -#include #include #include +#include -namespace ed -{ - -namespace io +namespace ed::io { // ---------------------------------------------------------------------------------------------------- @@ -26,10 +23,10 @@ enum NodeType struct Node { - Node() {} + Node() = default; Node(unsigned int idx_, NodeType type_) : idx(idx_), type(type_) {} - unsigned int idx; + unsigned int idx{}; NodeType type; }; @@ -37,17 +34,15 @@ struct Node struct Data { - std::vector > arrays; + std::vector> arrays; std::vector array_parents; - std::vector > maps; + std::vector> maps; std::vector map_parents; std::vector values; }; -} - -} +} // namespace ed::io #endif diff --git a/include/ed/io/data_writer.h b/include/ed/io/data_writer.h index bde36705..b46b9e63 100644 --- a/include/ed/io/data_writer.h +++ b/include/ed/io/data_writer.h @@ -5,22 +5,18 @@ #include -namespace ed -{ - -namespace io +namespace ed::io { class DataWriter { public: - DataWriter(Data& cfg, const Node& n = Node()) : data_(cfg), n_current_(n) { if (data_.maps.empty()) { - data_.maps.push_back(std::map()); + data_.maps.emplace_back(); data_.map_parents.push_back(-1); n_current_.idx = 0; n_current_.type = MAP; @@ -35,7 +31,7 @@ class DataWriter void writeGroup(const std::string& key) { - data_.maps.push_back(std::map()); + data_.maps.emplace_back(); data_.map_parents.push_back(n_current_.idx); data_.maps[n_current_.idx][key] = Node(data_.maps.size() - 1, MAP); @@ -53,7 +49,7 @@ class DataWriter void writeArray(const std::string& key) { - data_.arrays.push_back(std::vector()); + data_.arrays.emplace_back(); data_.array_parents.push_back(n_current_.idx); data_.maps[n_current_.idx][key] = Node(data_.arrays.size() - 1, ARRAY); @@ -76,10 +72,10 @@ class DataWriter std::vector& array = data_.arrays[n_current_.idx]; - data_.maps.push_back(std::map()); + data_.maps.emplace_back(); data_.map_parents.push_back(n_current_.idx); - array.push_back(Node(data_.maps.size() - 1, MAP)); + array.emplace_back(data_.maps.size() - 1, MAP); n_current_.idx = data_.maps.size() - 1; n_current_.type = MAP; @@ -95,14 +91,10 @@ class DataWriter } private: - Data& data_; Node n_current_; - }; -} - -} +} // namespace ed::io #endif diff --git a/include/ed/io/filesystem/read.h b/include/ed/io/filesystem/read.h index a34ddd53..ca95112c 100644 --- a/include/ed/io/filesystem/read.h +++ b/include/ed/io/filesystem/read.h @@ -13,6 +13,6 @@ bool read(const std::string& filename, Measurement& msr); bool readEntity(const std::string& filename, UpdateRequest& req); -} +} // namespace ed #endif diff --git a/include/ed/io/filesystem/write.h b/include/ed/io/filesystem/write.h index 0f905692..af669ff3 100644 --- a/include/ed/io/filesystem/write.h +++ b/include/ed/io/filesystem/write.h @@ -11,8 +11,8 @@ class Entity; bool write(const std::string& filename, const Measurement& msr); -bool write(const std::string &filename, const Entity& e); +bool write(const std::string& filename, const Entity& e); -} +} // namespace ed #endif diff --git a/include/ed/io/json_reader.h b/include/ed/io/json_reader.h index 8ae199da..2b34a1a1 100644 --- a/include/ed/io/json_reader.h +++ b/include/ed/io/json_reader.h @@ -1,72 +1,60 @@ #ifndef ED_IO_JSON_READER_H_ #define ED_IO_JSON_READER_H_ -#include "ed/io/reader.h" #include "ed/io/data.h" +#include "ed/io/reader.h" #include #include -namespace ed -{ - -namespace io +namespace ed::io { class JSONReader : public ed::io::Reader { public: - JSONReader(const char* s); - virtual ~JSONReader(); + ~JSONReader() override; - bool readGroup(const std::string& name); - bool endGroup(); + bool readGroup(const std::string& name) override; + bool endGroup() override; - bool readArray(const std::string& name); - bool endArray(); + bool readArray(const std::string& name) override; + bool endArray() override; - bool nextArrayItem(); + bool nextArrayItem() override; - bool readValue(const std::string&, float& f); - bool readValue(const std::string&, double& d); - bool readValue(const std::string&, int& i); - bool readValue(const std::string&, std::string& s); + bool readValue(const std::string&, float& f) override; + bool readValue(const std::string&, double& d) override; + bool readValue(const std::string&, int& i) override; + bool readValue(const std::string&, std::string& s) override; - bool ok() { return error_.empty(); } + bool ok() override { return error_.empty(); } - std::string error() { return error_; } + std::string error() override { return error_; } private: - Data data_; Node n_current_; std::vector array_index_stack_; std::string error_; - template - bool value(const std::string& key, T& value) const + template bool value(const std::string& key, T& value) const { const std::map& map = data_.maps[n_current_.idx]; - std::map::const_iterator it = map.find(key); + auto const it = map.find(key); if (it == map.end()) return false; const Variant& v = data_.values[it->second.idx]; - if (!v.getValue(value)) - return false; - - return true; + return static_cast(v.getValue(value)); } - }; -} - -} +} // namespace ed::io #endif diff --git a/include/ed/io/json_writer.h b/include/ed/io/json_writer.h index 47312e39..f66a0110 100644 --- a/include/ed/io/json_writer.h +++ b/include/ed/io/json_writer.h @@ -6,25 +6,18 @@ #include #include -namespace ed -{ - -namespace io +namespace ed::io { class JSONWriter : public Writer { public: + JSONWriter(std::ostream& out) : Writer(out) { out << "{"; } - JSONWriter(std::ostream& out) : Writer(out), add_comma_(false) - { - out << "{"; - } - - ~JSONWriter() {} + ~JSONWriter() override = default; - void writeGroup(const std::string& name) + void writeGroup(const std::string& name) override { if (add_comma_) out_ << ","; @@ -34,7 +27,7 @@ class JSONWriter : public Writer add_comma_ = false; } - void endGroup() + void endGroup() override { out_ << "}"; if (type_stack_.empty() || type_stack_.back() != 'g') @@ -44,7 +37,7 @@ class JSONWriter : public Writer add_comma_ = true; } - void writeValue(const std::string& key, float f) + void writeValue(const std::string& key, float f) override { if (add_comma_) out_ << ","; @@ -53,7 +46,7 @@ class JSONWriter : public Writer add_comma_ = true; } - void writeValue(const std::string& key, int i) + void writeValue(const std::string& key, int i) override { if (add_comma_) out_ << ","; @@ -62,7 +55,7 @@ class JSONWriter : public Writer add_comma_ = true; } - void writeValue(const std::string& key, const std::string& s) + void writeValue(const std::string& key, const std::string& s) override { if (add_comma_) out_ << ","; @@ -71,7 +64,7 @@ class JSONWriter : public Writer add_comma_ = true; } - void writeValue(const std::string& key, double d) + void writeValue(const std::string& key, double d) override { if (add_comma_) out_ << ","; @@ -80,7 +73,7 @@ class JSONWriter : public Writer add_comma_ = true; } - void writeValue(const std::string& key, const float* fs, std::size_t size) + void writeValue(const std::string& key, const float* fs, std::size_t size) override { if (add_comma_) out_ << ","; @@ -90,13 +83,13 @@ class JSONWriter : public Writer if (size > 0) { out_ << fs[0]; - for(unsigned int i = 1; i < size; ++i) + for (unsigned int i = 1; i < size; ++i) out_ << "," << fs[i]; } out_ << "]"; } - void writeValue(const std::string& key, const int* is, std::size_t size) + void writeValue(const std::string& key, const int* is, std::size_t size) override { if (add_comma_) out_ << ","; @@ -106,13 +99,13 @@ class JSONWriter : public Writer if (size > 0) { out_ << is[0]; - for(unsigned int i = 1; i < size; ++i) + for (unsigned int i = 1; i < size; ++i) out_ << "," << is[i]; } out_ << "]"; } - void writeValue(const std::string& key, const std::string* ss, std::size_t size) + void writeValue(const std::string& key, const std::string* ss, std::size_t size) override { if (add_comma_) out_ << ","; @@ -122,13 +115,13 @@ class JSONWriter : public Writer if (size > 0) { out_ << "\"" << ss[0] << "\""; - for(unsigned int i = 1; i < size; ++i) + for (unsigned int i = 1; i < size; ++i) out_ << "\"" << ss[i] << "\""; } out_ << "]"; } - void writeArray(const std::string& key) + void writeArray(const std::string& key) override { if (add_comma_) out_ << ","; @@ -138,7 +131,7 @@ class JSONWriter : public Writer add_comma_ = false; } - void addArrayItem() + void addArrayItem() override { if (add_comma_) out_ << ","; @@ -147,7 +140,7 @@ class JSONWriter : public Writer type_stack_.push_back('i'); add_comma_ = false; } - void endArrayItem() + void endArrayItem() override { out_ << "}"; if (type_stack_.empty() || type_stack_.back() != 'i') @@ -157,7 +150,7 @@ class JSONWriter : public Writer add_comma_ = true; } - void endArray() + void endArray() override { out_ << "]"; if (type_stack_.empty() || type_stack_.back() != 'a') @@ -167,11 +160,11 @@ class JSONWriter : public Writer add_comma_ = true; } - void finish() + void finish() override { - while(!type_stack_.empty()) + while (!type_stack_.empty()) { - char t = type_stack_.back(); + char const t = type_stack_.back(); type_stack_.pop_back(); if (t == 'g') @@ -185,14 +178,10 @@ class JSONWriter : public Writer } private: - - bool add_comma_; + bool add_comma_{false}; std::vector type_stack_; - }; -} - -} // end namespace era +} // namespace ed::io #endif diff --git a/include/ed/io/reader.h b/include/ed/io/reader.h index c86a16a4..9847da33 100644 --- a/include/ed/io/reader.h +++ b/include/ed/io/reader.h @@ -5,20 +5,16 @@ #include -namespace ed -{ - -namespace io +namespace ed::io { class Reader { public: + Reader() = default; - Reader() {} - - virtual ~Reader() {} + virtual ~Reader() = default; virtual bool readGroup(const std::string& name) = 0; virtual bool endGroup() = 0; @@ -36,12 +32,8 @@ class Reader virtual bool ok() = 0; virtual std::string error() = 0; - }; -} - -} // end namespace era +} // namespace ed::io #endif - diff --git a/include/ed/io/transport/probe.h b/include/ed/io/transport/probe.h index 35f99603..1819746d 100644 --- a/include/ed/io/transport/probe.h +++ b/include/ed/io/transport/probe.h @@ -7,10 +7,9 @@ #include #include -#include +#include -#include -#include +#include namespace ed { @@ -19,44 +18,44 @@ class Probe : public Plugin { public: - Probe(); - virtual ~Probe(); - + ~Probe() override; // Plugin interface - void initialize(); - - void process(const WorldModel& world, UpdateRequest& req); + void initialize() override; + void process(const WorldModel& world, UpdateRequest& req) override; // Probe interface - virtual void configure(tue::Configuration /*config*/) {} + void configure(tue::Configuration /*config*/) override {} using Plugin::process; virtual void process(const WorldModel& /*world*/, UpdateRequest& /*update*/, tue::serialization::InputArchive& /*req*/, - tue::serialization::OutputArchive& /*res*/) {} + tue::serialization::OutputArchive& /*res*/) + { + } private: + const ed::WorldModel* world_{}; + ed::UpdateRequest* update_req_{}; - const ed::WorldModel* world_; - ed::UpdateRequest* update_req_; - - ros::CallbackQueue cb_queue_; + rclcpp::CallbackGroup::SharedPtr cb_group_; - ros::ServiceServer srv_; + rclcpp::executors::SingleThreadedExecutor executor_; - bool srvCallback(const tue_serialization::BinaryService::Request& ros_req, - tue_serialization::BinaryService::Response& ros_res); + rclcpp::Service::SharedPtr srv_; + // NOLINTNEXTLINE(performance-unnecessary-value-param) - rclcpp service callback requires shared_ptr by value + void srvCallback(const std::shared_ptr ros_req, + const std::shared_ptr& ros_res); }; -} +} // namespace ed #endif diff --git a/include/ed/io/transport/probe_client.h b/include/ed/io/transport/probe_client.h index aabb7b0c..fc3ac00c 100644 --- a/include/ed/io/transport/probe_client.h +++ b/include/ed/io/transport/probe_client.h @@ -5,7 +5,8 @@ #include -#include +#include +#include namespace ed { @@ -14,29 +15,30 @@ class ProbeClient { public: - ProbeClient(); virtual ~ProbeClient(); void launchProbe(const std::string& probe_name, const std::string& lib); - void configure(tue::Configuration config); + void configure(const tue::Configuration& config); bool process(tue::serialization::Archive& req, tue::serialization::Archive& res); - const std::string& probeName() const { return probe_name_; } + [[nodiscard]] + const std::string& probeName() const + { + return probe_name_; + } private: - - ros::NodeHandle* nh_; + rclcpp::Node::SharedPtr node_; std::string probe_name_; - ros::ServiceClient srv_probe_; - + rclcpp::Client::SharedPtr srv_probe_; }; -} +} // namespace ed #endif diff --git a/include/ed/io/transport/probe_ros.h b/include/ed/io/transport/probe_ros.h index 5196b7e2..c18e1e1d 100644 --- a/include/ed/io/transport/probe_ros.h +++ b/include/ed/io/transport/probe_ros.h @@ -8,13 +8,11 @@ class ProbeROS { public: - ProbeROS(); virtual ~ProbeROS(); - }; -} +} // namespace ed #endif diff --git a/include/ed/io/variant.h b/include/ed/io/variant.h index ff95acc7..006de2c1 100644 --- a/include/ed/io/variant.h +++ b/include/ed/io/variant.h @@ -1,60 +1,67 @@ #ifndef ERA_TUE_CONFIGURATION_VARIANT_H_ #define ERA_TUE_CONFIGURATION_VARIANT_H_ -#include +#include #include #include -#include - -namespace ed -{ +#include +#include -namespace io +namespace ed::io { class Variant { public: - Variant() : type_('?') {} Variant(const double& d) : type_('d'), d_(d) {} Variant(int i) : type_('i'), i_(i) {} - Variant(const std::string& s) : type_('s'), s_(s) {} + Variant(std::string s) : type_('s'), s_(std::move(s)) {} Variant(const char* s) : type_('s'), s_(s) {} bool getValue(int& v) const { return checkAndGet(i_, 'i', v); } - bool getValue(double& v) const { return checkAndGet(d_, 'd', v) || checkAndGet((double)i_, 'i', v); } - bool getValue(float& v) const { return checkAndGet((float)d_, 'd', v) || checkAndGet((float)i_, 'i', v); } + bool getValue(double& v) const { return checkAndGet(d_, 'd', v) || checkAndGet(static_cast(i_), 'i', v); } + bool getValue(float& v) const + { + return checkAndGet(static_cast(d_), 'd', v) || checkAndGet(static_cast(i_), 'i', v); + } bool getValue(std::string& v) const { return checkAndGet(s_, 's', v); } bool getValue(bool& v) const { - int i; + int i = 0; if (!checkAndGet(i_, 'i', i)) return false; v = (i == 1); return true; } - bool isString() const { return type_ == 's'; } + [[nodiscard]] + bool isString() const + { + return type_ == 's'; + } - bool inline valid() const { return type_ != '?'; } + [[nodiscard]] + bool valid() const + { + return type_ != '?'; + } private: - char type_; - union { - int i_; + union + { + int i_{}; double d_; }; std::string s_; - template - inline bool checkAndGet(const T& v, char type, T& out) const + template bool checkAndGet(const T& v, char type, T& out) const { if (type != type_) return false; @@ -62,27 +69,20 @@ class Variant return true; } - friend std::ostream& operator<< (std::ostream& out, const Variant& v) + friend std::ostream& operator<<(std::ostream& out, const Variant& v) { switch (v.type_) { - case 'i': out << v.i_; - break; - case 'd': out << v.d_; - break; - case 's': out << v.s_; - break; - default: out << "?"; - break; + case 'i': out << v.i_; break; + case 'd': out << v.d_; break; + case 's': out << v.s_; break; + default: out << "?"; break; } return out; } - }; -} - -} +} // namespace ed::io #endif diff --git a/include/ed/io/writer.h b/include/ed/io/writer.h index da7f5d56..4f7c5682 100644 --- a/include/ed/io/writer.h +++ b/include/ed/io/writer.h @@ -4,24 +4,20 @@ #include #include -//#include "ed/io/data.h" +// #include "ed/io/data.h" #include -namespace ed -{ - -namespace io +namespace ed::io { class Writer { public: - Writer(std::ostream& out) : out_(out) {} - virtual ~Writer() {} + virtual ~Writer() = default; virtual void writeGroup(const std::string& name) = 0; virtual void endGroup() = 0; @@ -35,9 +31,18 @@ class Writer virtual void writeValue(const std::string& key, const int* is, std::size_t size) = 0; virtual void writeValue(const std::string& key, const std::string* ss, std::size_t size) = 0; - virtual void writeValue(const std::string& key, const std::vector& fs) { writeValue(key, &fs[0], fs.size()); } - virtual void writeValue(const std::string& key, const std::vector& is) { writeValue(key, &is[0], is.size()); } - virtual void writeValue(const std::string& key, const std::vector& ss) { writeValue(key, &ss[0], ss.size()); } + virtual void writeValue(const std::string& key, const std::vector& fs) + { + writeValue(key, fs.data(), fs.size()); + } + virtual void writeValue(const std::string& key, const std::vector& is) + { + writeValue(key, is.data(), is.size()); + } + virtual void writeValue(const std::string& key, const std::vector& ss) + { + writeValue(key, ss.data(), ss.size()); + } virtual void writeArray(const std::string& key) = 0; virtual void addArrayItem() = 0; @@ -47,14 +52,9 @@ class Writer virtual void finish() {} protected: - std::ostream& out_; - }; -} - -} // end namespace era +} // namespace ed::io #endif - diff --git a/include/ed/logging.h b/include/ed/logging.h index fa998fb2..1cfa2286 100644 --- a/include/ed/logging.h +++ b/include/ed/logging.h @@ -4,10 +4,7 @@ #include #include -namespace ed -{ - -namespace log +namespace ed::log { std::ostream& info(); @@ -28,8 +25,6 @@ void error(const char* str); void error(const std::string& str); -} - -} +} // namespace ed::log #endif diff --git a/include/ed/loop_usage_status.h b/include/ed/loop_usage_status.h index 53eb103b..cb937c10 100644 --- a/include/ed/loop_usage_status.h +++ b/include/ed/loop_usage_status.h @@ -1,28 +1,34 @@ #ifndef ED_LOOP_USAGE_STATUS_H_ #define ED_LOOP_USAGE_STATUS_H_ +#if __has_include() +#include +#include +#else #include #include +#endif -#include +#include #include #include -#include +#include +#include namespace ed { - /** - * @brief A diagnostic task that monitors the frequency of an event. - * - * This diagnostic task monitors the frequency and usage of a loop and creates corresponding diagnostics. - * It will report a warning if the frequency is outside acceptable bounds, and report an error - * if there have been no events in the latest window. - * Heavily inspired by diagnostic_updater::FrequencyStatus - */ +/** + * @brief A diagnostic task that monitors the frequency of an event. + * + * This diagnostic task monitors the frequency and usage of a loop and creates corresponding diagnostics. + * It will report a warning if the frequency is outside acceptable bounds, and report an error + * if there have been no events in the latest window. + * Heavily inspired by diagnostic_updater::FrequencyStatus + */ class LoopUsageStatus : public diagnostic_updater::DiagnosticTask { @@ -33,9 +39,9 @@ class LoopUsageStatus : public diagnostic_updater::DiagnosticTask std::vector starts_; // Micro-seconds std::vector durations_; // Seconds std::vector seq_nums_; - int hist_indx_; - long double start_; - long double duration_; + int hist_indx_{}; + long double start_{}; + long double duration_{}; boost::mutex lock_; @@ -43,9 +49,9 @@ class LoopUsageStatus : public diagnostic_updater::DiagnosticTask /** * @brief Constructs a LoopUsageStatus class with the given parameters. */ - LoopUsageStatus(const diagnostic_updater::FrequencyStatusParam ¶ms, std::string name) : - DiagnosticTask(name), params_(params), - starts_(params_.window_size_), durations_(params_.window_size_), seq_nums_(params_.window_size_) + LoopUsageStatus(const diagnostic_updater::FrequencyStatusParam& params, std::string name) : + DiagnosticTask(name), params_(params), starts_(params_.window_size_), durations_(params_.window_size_), + seq_nums_(params_.window_size_) { clear(); } @@ -55,27 +61,32 @@ class LoopUsageStatus : public diagnostic_updater::DiagnosticTask * Uses a default diagnostic task name of "Loop Usage Status". */ - LoopUsageStatus(const diagnostic_updater::FrequencyStatusParam ¶ms) : - DiagnosticTask("Loop Usage Status"), params_(params), - starts_(params_.window_size_), durations_(params_.window_size_), seq_nums_(params_.window_size_) + LoopUsageStatus(const diagnostic_updater::FrequencyStatusParam& params) : + DiagnosticTask("Loop Usage Status"), params_(params), starts_(params_.window_size_), + durations_(params_.window_size_), seq_nums_(params_.window_size_) { clear(); } /** - * @brief Expose const reference to loop timer. No call to the const timer will be able to influence the behaviour of this class. + * @brief Expose const reference to loop timer. No call to the const timer will be able to influence the behaviour + * of this class. * @return const reference to the loop timer */ - const tue::LoopTimer& getTimer() const { return timer_; } + [[nodiscard]] + const tue::LoopTimer& getTimer() const + { + return timer_; + } /** * @brief Resets the statistics. */ void clear() { - boost::mutex::scoped_lock lock(lock_); + boost::mutex::scoped_lock const lock(lock_); timer_.reset(); - long double curtime = tue::Timer::nowMicroSec(); + long double const curtime = tue::Timer::nowMicroSec(); std::fill(starts_.begin(), starts_.end(), curtime); std::fill(durations_.begin(), durations_.end(), 0); @@ -89,7 +100,7 @@ class LoopUsageStatus : public diagnostic_updater::DiagnosticTask */ void start() { - boost::mutex::scoped_lock lock(lock_); + boost::mutex::scoped_lock const lock(lock_); timer_.start(); } @@ -98,22 +109,22 @@ class LoopUsageStatus : public diagnostic_updater::DiagnosticTask */ void stop() { - boost::mutex::scoped_lock lock(lock_); + boost::mutex::scoped_lock const lock(lock_); timer_.stop(); } /** * @brief Fills out this Task's DiagnosticStatusWrapper. */ - virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat) + void run(diagnostic_updater::DiagnosticStatusWrapper& stat) override { - boost::mutex::scoped_lock lock(lock_); - long double curtime = tue::Timer::nowMicroSec(); // Micro-seconds - long double total_loop_time = timer_.getTotalLoopTime(); // Seconds - int curseq = timer_.getIterationCount(); - int events = curseq - seq_nums_[hist_indx_]; - double window = (curtime - starts_[hist_indx_]) * 0.000001; // Micro-seconds -> Seconds - double loop_time_window = total_loop_time - durations_[hist_indx_]; // Seconds + boost::mutex::scoped_lock const lock(lock_); + long double const curtime = tue::Timer::nowMicroSec(); // Micro-seconds + long double const total_loop_time = timer_.getTotalLoopTime(); // Seconds + int const curseq = timer_.getIterationCount(); + int const events = curseq - seq_nums_[hist_indx_]; + double const window = (curtime - starts_[hist_indx_]) * 0.000001; // Micro-seconds -> Seconds + double const loop_time_window = total_loop_time - durations_[hist_indx_]; // Seconds double freq = 0; if (window != 0) @@ -127,19 +138,19 @@ class LoopUsageStatus : public diagnostic_updater::DiagnosticTask if (events == 0) { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No events recorded."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No events recorded."); } else if (window != 0 && freq < *params_.min_freq_ * (1 - params_.tolerance_)) { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Frequency too low."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Frequency too low."); } else if (window != 0 && freq > *params_.max_freq_ * (1 + params_.tolerance_)) { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Frequency too high."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Frequency too high."); } else if (window != 0) { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Desired frequency met"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Desired frequency met"); } stat.addf("Events in window", "%d", events); @@ -148,21 +159,21 @@ class LoopUsageStatus : public diagnostic_updater::DiagnosticTask stat.addf("Total loop time during window (s)", "%f", loop_time_window); if (window != 0) { - stat.addf("Loop Usage (%)", "%f", 100*loop_time_window/window); + stat.addf("Loop Usage (%)", "%f", 100 * loop_time_window / window); stat.addf("Actual frequency (Hz)", "%f", freq); } if (*params_.min_freq_ == *params_.max_freq_) { - stat.addf("Target frequency (Hz)", "%f",*params_.min_freq_); + stat.addf("Target frequency (Hz)", "%f", *params_.min_freq_); } if (*params_.min_freq_ > 0) { - stat.addf("Minimum acceptable frequency (Hz)", "%f", *params_.min_freq_ * (1 - params_.tolerance_)); + stat.addf("Minimum acceptable frequency (Hz)", "%f", *params_.min_freq_ * (1 - params_.tolerance_)); } if (std::isfinite(*params_.max_freq_)) { - stat.addf("Maximum acceptable frequency (Hz)", "%f", *params_.max_freq_ * (1 + params_.tolerance_)); + stat.addf("Maximum acceptable frequency (Hz)", "%f", *params_.max_freq_ * (1 + params_.tolerance_)); } } }; diff --git a/include/ed/mask.h b/include/ed/mask.h index afebe9d5..0a1e9212 100644 --- a/include/ed/mask.h +++ b/include/ed/mask.h @@ -1,106 +1,104 @@ #ifndef ED_MASK_H_ #define ED_MASK_H_ -#include #include +#include -#include -#include #include -#include #include +#include +#include +#include namespace ed { /** Support for scanning an image as a number of sub-image scans. */ -class ImageMask { +class ImageMask +{ public: - - ImageMask() {} + ImageMask() = default; /** * Construct the mask, while setting the mask size. * @param width Width of the mask. * @param height Height of the mask. */ - ImageMask(int width, int height) : width_(width), height_(height) - { - } + ImageMask(int width, int height) : width_(width), height_(height) {} /** * Set the size of the image. * @param width Width of the mask. * @param height Height of the mask. */ - inline void setSize(int width, int height) + void setSize(int width, int height) { width_ = width; height_ = height; } /** Remove all sub-images. */ - inline void clear() - { - points_.clear(); - } + void clear() { points_.clear(); } /** * Get the number of sub-images. * @return The number of sub-images in the mask. */ - inline int getSize() const { return points_.size(); } + [[nodiscard]] + int getSize() const + { + return points_.size(); + } /** * Get the width of the mask. * @return The width of the mask. */ - inline int width() const { return width_; } + [[nodiscard]] + int width() const + { + return width_; + } /** * Get the height of the mask. * @return The height of the mask. */ - inline int height() const { return height_; } + [[nodiscard]] + int height() const + { + return height_; + } /** * Add a sub-image. * @param p Base-point of the new sub-image. */ - inline void addPoint(const cv::Point2i& p) - { - points_.push_back(p); - } + void addPoint(const cv::Point2i& p) { points_.push_back(p); } /** * Add a sub-image. * @param x X coordinate of the new sub-image. * @param y Y coordinate of the new sub-image. */ - inline void addPoint(int x, int y) - { - addPoint(cv::Point2i(x, y)); - } + void addPoint(int x, int y) { addPoint(cv::Point2i(x, y)); } /** * Add a one-pixel sub-image. * @param idx Index number of the pixel (scanning horizontally, from top to bottom). */ - inline void addPoint(int idx) - { - addPoint(idx % width_, idx / width_); - } + void addPoint(int idx) { addPoint(idx % width_, idx / width_); } /** * Add a number of sub-images. * @param ps Base points of the new sub-images. */ - inline void addPoints(const std::vector& ps) + void addPoints(const std::vector& ps) { - for(std::vector::const_iterator it = ps.begin(); it != ps.end(); ++it) + for (auto p : ps) { - addPoint(*it); + addPoint(p); } } @@ -118,13 +116,13 @@ class ImageMask { * @param index Index of the first sub-image to scan. * @param factor Size of a rectangular sub-image. */ - const_iterator(const std::vector &points, size_t index, int factor) - : points_(points), index_(index), dx_(0), dy_(0), factor_(factor) + const_iterator(const std::vector& points, size_t index, int factor) : + points_(points), index_(index), factor_(factor) { } // post increment operator - inline const_iterator operator++(int) + const_iterator operator++(int) { const_iterator i(*this); ++*this; @@ -137,7 +135,7 @@ class ImageMask { * the next scan line, jump to the next sub-image at the end of the * current sub-image. */ - inline const_iterator& operator++() + const_iterator& operator++() { ++dx_; @@ -159,24 +157,25 @@ class ImageMask { } /** Compute the xy mask position of the iterator. */ - inline cv::Point2i operator()() + cv::Point2i operator()() { - const cv::Point2i &pt = points_[index_]; - return cv::Point2i(pt.x * factor_ + dx_, pt.y * factor_ + dy_); + const cv::Point2i& pt = points_[index_]; + return {(pt.x * factor_) + dx_, (pt.y * factor_) + dy_}; } // Note: iterator equality only checks base-point index, and not dx/dy // sub-image position. - inline bool operator==(const const_iterator& rhs) { return index_ == rhs.index_; } - inline bool operator!=(const const_iterator& rhs) { return index_ != rhs.index_; } - private: + bool operator==(const const_iterator& rhs) const { return index_ == rhs.index_; } + bool operator!=(const const_iterator& rhs) const { return index_ != rhs.index_; } - const std::vector &points_; ///< Base points of the sub-images. - size_t index_; ///< Current sub-image being scanned. - int dx_, dy_; ///< Variables tracking the x/y position in the current sub-image. - int factor_; ///< Sub-image X/Y size (sub-image is rectangular). + private: + const std::vector& points_; ///< Base points of the sub-images. + size_t index_; ///< Current sub-image being scanned. + int dx_{0}, dy_{0}; ///< Variables tracking the x/y position in the current sub-image. + int factor_; ///< Sub-image X/Y size (sub-image is rectangular). }; + [[nodiscard]] const_iterator begin(int width = 0) const { if (width <= 0) @@ -185,14 +184,15 @@ class ImageMask { return const_iterator(points_, 0, width / width_); } + [[nodiscard]] const_iterator end() const { return const_iterator(points_, points_.size(), 0); } private: - int width_; ///< Width of the mask. - int height_; ///< Height of the mask. + int width_{}; ///< Width of the mask. + int height_{}; ///< Height of the mask. std::vector points_; ///< Base points of the sub-images. }; diff --git a/include/ed/measurement.h b/include/ed/measurement.h index 333c4ed6..4a968452 100644 --- a/include/ed/measurement.h +++ b/include/ed/measurement.h @@ -1,9 +1,9 @@ #ifndef measurement_h_ #define measurement_h_ -#include "ed/types.h" #include "ed/mask.h" #include "ed/rgbd_data.h" +#include "ed/types.h" namespace ed { @@ -12,29 +12,46 @@ class Measurement { public: - Measurement(); - Measurement(rgbd::ImageConstPtr image, const ImageMask& image_mask, const geo::Pose3D& sensor_pose); - - Measurement(const RGBDData& rgbd_data, const PointCloudMaskPtr& mask, unsigned int seq = 0); - - const geo::Pose3D& sensorPose() const { return rgbd_data_.sensor_pose; } - rgbd::ImageConstPtr image() const { return rgbd_data_.image; } - PointCloudMaskConstPtr mask() const { return mask_; } - const ImageMask& imageMask() const { return image_mask_; } - double timestamp() const { return timestamp_; } + Measurement(const rgbd::ImageConstPtr& image, ImageMask image_mask, const geo::Pose3D& sensor_pose); + + Measurement(const RGBDData& rgbd_data, PointCloudMaskPtr mask, unsigned int seq = 0); + + [[nodiscard]] + const geo::Pose3D& sensorPose() const + { + return rgbd_data_.sensor_pose; + } + [[nodiscard]] + rgbd::ImageConstPtr image() const + { + return rgbd_data_.image; + } + [[nodiscard]] + PointCloudMaskConstPtr mask() const + { + return mask_; + } + [[nodiscard]] + const ImageMask& imageMask() const + { + return image_mask_; + } + [[nodiscard]] + double timestamp() const + { + return timestamp_; + } protected: - RGBDData rgbd_data_; PointCloudMaskPtr mask_; ImageMask image_mask_; double timestamp_; - unsigned int seq_; - + unsigned int seq_{}; }; -} +} // namespace ed #endif diff --git a/include/ed/measurement_convex_hull.h b/include/ed/measurement_convex_hull.h index 4928c53e..d9c31ef6 100644 --- a/include/ed/measurement_convex_hull.h +++ b/include/ed/measurement_convex_hull.h @@ -11,9 +11,9 @@ struct MeasurementConvexHull { ConvexHull convex_hull; geo::Pose3D pose; - double timestamp; + double timestamp{}; }; -} +} // namespace ed #endif diff --git a/include/ed/models/model_loader.h b/include/ed/models/model_loader.h index 5f2fe195..2fddcfb1 100644 --- a/include/ed/models/model_loader.h +++ b/include/ed/models/model_loader.h @@ -9,17 +9,13 @@ #include #include -namespace ed -{ - -namespace models +namespace ed::models { class ModelLoader { public: - ModelLoader(); ~ModelLoader(); @@ -34,7 +30,11 @@ class ModelLoader * @param allow_sdf Allow SDF models, or only ED yaml models * @return bool, which indicates succes */ - bool create(const UUID& id, const std::string& type, UpdateRequest& req, std::stringstream& error, const bool allow_sdf=false); + bool create(const UUID& id, + const std::string& type, + UpdateRequest& req, + std::stringstream& error, + const bool allow_sdf = false); /** * @brief create add entity to update_request from config data. "_root" will be used as id. @@ -56,8 +56,12 @@ class ModelLoader * @param pose_offset pose offset, if no pose in data, this pose will be the final pose. * @return bool, which indicates succes */ - bool create(const tue::config::DataConstPointer& data, const UUID& id_opt, const UUID& parent_id, - UpdateRequest& req, std::stringstream& error, const std::string& model_path = "", + bool create(const tue::config::DataConstPointer& data, + const UUID& id_opt, + const UUID& parent_id, + UpdateRequest& req, + std::stringstream& error, + const std::string& model_path = "", const geo::Pose3D& pose_offset = geo::Pose3D::identity()); /** @@ -71,20 +75,24 @@ class ModelLoader * @param error error stream * @return bool, which indicates succes */ - bool createSDF(const tue::config::DataConstPointer& data, const UUID& parent_id, const geo::Pose3D& parent_pose, - const UUID& id_override, const boost::shared_ptr pose_override, - UpdateRequest& req, std::stringstream& error); + bool createSDF(const tue::config::DataConstPointer& data, + const UUID& parent_id, + const geo::Pose3D& parent_pose, + const UUID& id_override, + const boost::shared_ptr& pose_override, + UpdateRequest& req, + std::stringstream& error); /** * @brief exists Check of a model of type 'type' exist * @param type model type * @return bool, which indicates of type exist */ + [[nodiscard]] bool exists(const std::string& type) const; private: - - typedef std::pair > ModelData; + using ModelData = std::pair>; // Model name to model data std::map model_cache_; @@ -105,8 +113,10 @@ class ModelLoader * @param allow_sdf Allow SDF models, or only ED yaml models * @return DataConstPointer with the data, empty in case of error */ - tue::config::DataConstPointer loadModelData(std::string type, std::vector& types, - std::stringstream& error, const bool allow_sdf=false); + tue::config::DataConstPointer loadModelData(const std::string& type, + std::vector& types, + std::stringstream& error, + const bool allow_sdf = false); /** * @brief loadSDFData load data of SDF model of uri 'uri' @@ -114,14 +124,14 @@ class ModelLoader * @param error error stream * @return DataConstPointer with the data, empty in case of error */ - tue::config::DataConstPointer loadSDFData(std::string uri, std::stringstream& error); - + tue::config::DataConstPointer loadSDFData(const std::string& uri, std::stringstream& error); /** * @brief getModelPath get file path of model of type 'type' * @param type type of the model * @return path of the model, empty if model not found */ + [[nodiscard]] std::string getModelPath(const std::string& type) const; /** @@ -129,6 +139,7 @@ class ModelLoader * @param uri uri of the model * @return path of the model, empty if model not found */ + [[nodiscard]] std::string getSDFPath(const std::string& uri) const; /** @@ -136,22 +147,20 @@ class ModelLoader * @param type type of the model to look for * @return ModelData, DataPointer is empty in case type not in cache */ - ModelData readModelCache(std::string type) const; - + [[nodiscard]] + ModelData readModelCache(const std::string& type) const; }; - /** * @brief The LoadType enum indicates whether to load directly from a file * or from a model that is part of the ED_MODEL_PATH */ enum class LoadType { - FILE, - MODEL, + FILE, + MODEL, }; - /** * @brief loadModel loads an ED model from file * @param load_type indicates whether the provided source is a filename or an identifier @@ -162,8 +171,8 @@ enum class LoadType */ bool loadModel(const LoadType load_type, const std::string& source, ed::UpdateRequest& req); -} // end namespace models +} // namespace ed::models -} // end namespace ed +// end namespace ed #endif diff --git a/include/ed/models/shape_loader.h b/include/ed/models/shape_loader.h index 7fd216bc..677aa3a2 100644 --- a/include/ed/models/shape_loader.h +++ b/include/ed/models/shape_loader.h @@ -8,13 +8,9 @@ #include #include -namespace ed +namespace ed::models { -namespace models -{ - - /** * @brief createCylinder create a mesh from radius and height * @param shape filled mesh @@ -25,8 +21,8 @@ namespace models void createCylinder(geo::Shape& shape, double radius, double height, int num_corners = 12); /** - * @brief getMiddlePoint Gets the middle point of two points in a mesh of a sphere. Uses a cache to not create double points. - * The new point is placed on the radius of the sphere. + * @brief getMiddlePoint Gets the middle point of two points in a mesh of a sphere. Uses a cache to not create double + * points. The new point is placed on the radius of the sphere. * @param mesh Mesh of the sphere * @param i1 index of first point * @param i2 index of second point @@ -44,8 +40,8 @@ uint getMiddlePoint(geo::Mesh& mesh, uint i1, uint i2, std::map -#define ED_REGISTER_PLUGIN(Derived) PLUGINLIB_EXPORT_CLASS(Derived, ed::Plugin) +#include +#define ED_REGISTER_PLUGIN(Derived) PLUGINLIB_EXPORT_CLASS(Derived, ed::Plugin) #include -#include "ed/types.h" #include "ed/init_data.h" +#include "ed/types.h" +#include #include #include - -namespace ed { +namespace ed +{ struct PluginInput { - PluginInput(const WorldModel& world_, const std::vector& deltas_) - : world(world_), deltas(deltas_) {} + PluginInput(const WorldModel& world_, const std::vector& deltas_) : + world(world_), deltas(deltas_) + { + } const WorldModel& world; const std::vector& deltas; @@ -31,8 +34,7 @@ class Plugin friend class PluginContainer; public: - - virtual ~Plugin() {} + virtual ~Plugin() = default; // Old virtual void configure(tue::Configuration /*config*/) {} @@ -43,18 +45,22 @@ class Plugin virtual void initialize(InitData& /*init*/) {} virtual void process(const PluginInput& /*data*/, UpdateRequest& /*req*/) {} - const std::string& name() const { return name_; } + [[nodiscard]] + const std::string& name() const + { + return name_; + } protected: - TFBufferConstPtr tf_buffer_; -private: + //! Shared node handle, set by the PluginContainer before configure()/initialize() + rclcpp::Node::SharedPtr node_; +private: std::string name_; - }; -} +} // namespace ed #endif diff --git a/include/ed/plugin_container.h b/include/ed/plugin_container.h index 10d7058e..9c561f66 100644 --- a/include/ed/plugin_container.h +++ b/include/ed/plugin_container.h @@ -7,15 +7,14 @@ #include +#include + #include #include #include -namespace pluginlib { - template - class ClassLoader; -} +namespace pluginlib { template class ClassLoader; } namespace ed { @@ -26,8 +25,7 @@ class PluginContainer { public: - - PluginContainer(const ed::TFBufferConstPtr& tf_buffer_); + PluginContainer(const rclcpp::Node::SharedPtr& node, const ed::TFBufferConstPtr& tf_buffer); virtual ~PluginContainer(); @@ -45,29 +43,34 @@ class PluginContainer UpdateRequestConstPtr updateRequest() const { - boost::lock_guard lg(mutex_update_request_); + boost::lock_guard const lg(mutex_update_request_); return update_request_; } void clearUpdateRequest() { - boost::lock_guard lg(mutex_update_request_); + boost::lock_guard const lg(mutex_update_request_); update_request_.reset(); } void setWorld(const WorldModelConstPtr& world) { - boost::lock_guard lg(mutex_world_); + boost::lock_guard const lg(mutex_world_); world_new_ = world; } - void setLoopFrequency(double freq) { loop_frequency_ = freq; loop_frequency_max_ = 1.05*freq; loop_frequency_min_= 0.9*freq; } // Magic numbers; Higher bound is stricter as it shouldn't be possible to exceed the desired frequency. + void setLoopFrequency(double freq) + { + loop_frequency_ = freq; + loop_frequency_max_ = 1.05 * freq; + loop_frequency_min_ = 0.9 * freq; + } // Magic numbers; Higher bound is stricter as it shouldn't be possible to exceed the desired frequency. double loopFrequency() const { return loop_frequency_; } void addDelta(const UpdateRequestConstPtr& delta) { - boost::lock_guard lg(mutex_world_); + boost::lock_guard const lg(mutex_world_); world_deltas_.push_back(delta); } @@ -76,24 +79,23 @@ class PluginContainer ed::LoopUsageStatus& getLoopUsageStatus() { return *loop_usage_status_; } protected: - - pluginlib::ClassLoader* class_loader_; + pluginlib::ClassLoader* class_loader_{nullptr}; PluginPtr plugin_; std::string name_; - bool request_stop_; + bool request_stop_{false}; - bool is_running_; + bool is_running_{false}; // 1.0 / cycle frequency - double cycle_duration_; + double cycle_duration_{0.1}; - double loop_frequency_; + double loop_frequency_{10}; - double loop_frequency_max_; - double loop_frequency_min_; + double loop_frequency_max_{11}; + double loop_frequency_min_{9}; mutable boost::mutex mutex_update_request_; @@ -101,9 +103,9 @@ class PluginContainer ed::shared_ptr thread_; - bool step_finished_; + bool step_finished_{true}; - double t_last_update_; + double t_last_update_{0}; mutable boost::mutex mutex_world_; @@ -113,18 +115,18 @@ class PluginContainer TFBufferConstPtr tf_buffer_; + rclcpp::Node::SharedPtr node_; + std::unique_ptr loop_usage_status_; bool step(); void run(); - // buffer of delta's since last process call std::vector world_deltas_; - }; -} +} // namespace ed #endif diff --git a/include/ed/properties/pose_info.h b/include/ed/properties/pose_info.h index 198eb3e8..8f788824 100644 --- a/include/ed/properties/pose_info.h +++ b/include/ed/properties/pose_info.h @@ -7,7 +7,6 @@ class PoseInfo : public ed::PropertyInfo { public: - void serialize(const ed::Variant& v, ed::io::Writer& w) const { const geo::Pose3D& p = v.getValue(); @@ -62,7 +61,6 @@ class PoseInfo : public ed::PropertyInfo } bool serializable() const { return true; } - }; #endif diff --git a/include/ed/property.h b/include/ed/property.h index 9bbf2b55..8f4cc17e 100644 --- a/include/ed/property.h +++ b/include/ed/property.h @@ -7,17 +7,17 @@ namespace ed { -class PropertyKeyDBEntry; +struct PropertyKeyDBEntry; struct Property { - Property() : entry(0), revision(-1) {} + Property() = default; Variant value; - const PropertyKeyDBEntry* entry; - unsigned long revision; + const PropertyKeyDBEntry* entry = nullptr; + unsigned long revision = -1; }; -} // end namespace +} // namespace ed #endif diff --git a/include/ed/property_info.h b/include/ed/property_info.h index ee69da8d..672b5f4c 100644 --- a/include/ed/property_info.h +++ b/include/ed/property_info.h @@ -1,10 +1,10 @@ #ifndef ED_PROPERTY_INFO_H_ #define ED_PROPERTY_INFO_H_ +#include "ed/io/reader.h" +#include "ed/io/writer.h" #include "ed/types.h" #include "ed/variant.h" -#include "ed/io/writer.h" -#include "ed/io/reader.h" namespace ed { @@ -13,23 +13,23 @@ class PropertyInfo { public: + PropertyInfo() = default; - PropertyInfo() {} + virtual ~PropertyInfo() = default; - virtual ~PropertyInfo() {} - - virtual void serialize(const Variant& /*v*/, io::Writer& /*out*/) const { } + virtual void serialize(const Variant& /*v*/, io::Writer& /*out*/) const {} virtual bool deserialize(io::Reader& /*in*/, Variant& /*v*/) const { return false; } - virtual bool serializable() const { return false; } + [[nodiscard]] + virtual bool serializable() const + { + return false; + } private: - - - }; -} // end namespace +} // namespace ed #endif diff --git a/include/ed/property_key.h b/include/ed/property_key.h index 994d5968..97905d63 100644 --- a/include/ed/property_key.h +++ b/include/ed/property_key.h @@ -6,19 +6,22 @@ namespace ed { -class PropertyKeyDBEntry; +struct PropertyKeyDBEntry; -template -struct PropertyKey +template struct PropertyKey { - PropertyKey() : idx(INVALID_IDX), entry(0) {} - Idx idx; + PropertyKey() = default; + Idx idx{INVALID_IDX}; - const PropertyKeyDBEntry* entry; + const PropertyKeyDBEntry* entry{nullptr}; - bool valid() const { return idx != INVALID_IDX; } + [[nodiscard]] + bool valid() const + { + return idx != INVALID_IDX; + } }; -} // end namespace +} // namespace ed #endif diff --git a/include/ed/property_key_db.h b/include/ed/property_key_db.h index 3c909d0e..19198a47 100644 --- a/include/ed/property_key_db.h +++ b/include/ed/property_key_db.h @@ -1,9 +1,9 @@ #ifndef ED_PROPERTY_KEY_DB_H_ #define ED_PROPERTY_KEY_DB_H_ -#include "ed/types.h" -#include "ed/property_key.h" #include "ed/property_info.h" +#include "ed/property_key.h" +#include "ed/types.h" #include @@ -12,39 +12,34 @@ namespace ed struct PropertyKeyDBEntry { - PropertyKeyDBEntry() : info(nullptr) {} + PropertyKeyDBEntry() = default; - ~PropertyKeyDBEntry() - { - if (info) - delete info; - } + ~PropertyKeyDBEntry() { delete info; } std::string name; - PropertyInfo* info; - Idx idx; + PropertyInfo* info{nullptr}; + Idx idx{}; }; class PropertyKeyDB { public: - ~PropertyKeyDB() { - for(std::map::iterator it = name_to_info_.begin(); it != name_to_info_.end(); ++it) + for (auto& it : name_to_info_) { - if(it->second) - delete it->second; + + delete it.second; } } - template - void registerProperty(const std::string& name, PropertyKey& key, PropertyInfo* info = 0) + template + void registerProperty(const std::string& name, PropertyKey& key, PropertyInfo* info = nullptr) { - PropertyKeyDBEntry* entry; + PropertyKeyDBEntry* entry = nullptr; - std::map::iterator it = name_to_info_.find(name); + auto const it = name_to_info_.find(name); if (it == name_to_info_.end()) { entry = new PropertyKeyDBEntry; @@ -74,21 +69,20 @@ class PropertyKeyDB key.idx = entry->idx; } + [[nodiscard]] const PropertyKeyDBEntry* getPropertyKeyDBEntry(const std::string& name) const { - std::map::const_iterator it = name_to_info_.find(name); + auto const it = name_to_info_.find(name); if (it == name_to_info_.end()) - return 0; + return nullptr; return it->second; } private: - std::map name_to_info_; - }; -} // end namespace +} // namespace ed #endif diff --git a/include/ed/relation.h b/include/ed/relation.h index ad4f592a..c8686197 100644 --- a/include/ed/relation.h +++ b/include/ed/relation.h @@ -1,8 +1,8 @@ #ifndef ED_RELATION_H_ #define ED_RELATION_H_ -#include "ed/types.h" #include "ed/time.h" +#include "ed/types.h" #include @@ -13,14 +13,11 @@ class Relation { public: - virtual bool calculateTransform(const Time& /*t*/, geo::Pose3D& /*tf*/) const { return false; } private: - -// Idx parent_idx_; -// Idx child_idx_; - + // Idx parent_idx_; + // Idx child_idx_; }; } // end namespace ed diff --git a/include/ed/relations/transform_cache.h b/include/ed/relations/transform_cache.h index 64ddc19c..13167930 100644 --- a/include/ed/relations/transform_cache.h +++ b/include/ed/relations/transform_cache.h @@ -11,20 +11,17 @@ class TransformCache : public ed::Relation { public: - TransformCache(); ~TransformCache(); - bool calculateTransform(const Time& t, geo::Pose3D& tf) const; + bool calculateTransform(const Time& t, geo::Pose3D& tf) const override; void insert(const Time& t, const geo::Pose3D& tf) { cache_.insert(t, tf); } private: - // Transforms, ordered in time TimeCache cache_; - }; } // end namespace ed diff --git a/include/ed/rendering.h b/include/ed/rendering.h index 646febfa..beae8328 100644 --- a/include/ed/rendering.h +++ b/include/ed/rendering.h @@ -4,15 +4,9 @@ #include // Forward declarations -namespace ed { - class WorldModel; -} -namespace geo { - class DepthCamera; -} -namespace cv { - class Mat; -} +namespace ed { class WorldModel; } +namespace geo { class DepthCamera; } +namespace cv { class Mat; } namespace ed { @@ -39,11 +33,14 @@ enum ShowVolumes * @param flatten Flatten all the meshes to the groundplane (default: false) * @return */ -bool renderWorldModel(const ed::WorldModel& world_model, const enum ShowVolumes show_volumes, - const geo::DepthCamera& cam, const geo::Pose3D& cam_pose_inv, - cv::Mat& depth_image, cv::Mat& image, bool flatten = false); - -} // End of namespace ed - +bool renderWorldModel(const ed::WorldModel& world_model, + const enum ShowVolumes show_volumes, + const geo::DepthCamera& cam, + const geo::Pose3D& cam_pose_inv, + cv::Mat& depth_image, + cv::Mat& image, + bool flatten = false); + +} // End of namespace ed #endif // RENDERING_H diff --git a/include/ed/rgbd_data.h b/include/ed/rgbd_data.h index 812b715f..61488863 100644 --- a/include/ed/rgbd_data.h +++ b/include/ed/rgbd_data.h @@ -1,23 +1,23 @@ #ifndef ED_RGBD_DATA_H_ #define ED_RGBD_DATA_H_ -#include -#include #include #include +#include +#include #include namespace ed { -typedef std::vector PointCloudMask; -typedef pcl::IndicesPtr PointCloudMaskPtr; -typedef pcl::IndicesConstPtr PointCloudMaskConstPtr; -typedef std::vector > PointCloudToPixelsMapping; +using PointCloudMask = std::vector; +using PointCloudMaskPtr = pcl::IndicesPtr; +using PointCloudMaskConstPtr = pcl::IndicesConstPtr; +using PointCloudToPixelsMapping = std::vector>; // TODO: check if this works! -const static PointCloudMask NO_MASK( 1, -1 ); +const static PointCloudMask NO_MASK(1, -1); struct RGBDData { @@ -36,6 +36,6 @@ struct RGBDData PointCloudToPixelsMapping point_cloud_to_pixels_mapping; }; -} +} // namespace ed #endif diff --git a/include/ed/serialization/archive.h b/include/ed/serialization/archive.h index c5c7ea99..00d520af 100644 --- a/include/ed/serialization/archive.h +++ b/include/ed/serialization/archive.h @@ -6,54 +6,79 @@ namespace ed { -class OArchive { +class OArchive +{ public: - - OArchive(std::ostream& stream, int version) : stream_(stream) { - stream_.write((char*)&version, sizeof(version)); - } + OArchive(std::ostream& stream, int version) : stream_(stream) { stream_.write((char*)&version, sizeof(version)); } virtual ~OArchive() {} - inline OArchive& operator<<(float f) { stream_.write((char*)&f, sizeof(f)); return *this; } + inline OArchive& operator<<(float f) + { + stream_.write((char*)&f, sizeof(f)); + return *this; + } - inline OArchive& operator<<(double d) { stream_.write((char*)&d, sizeof(d)); return *this; } + inline OArchive& operator<<(double d) + { + stream_.write((char*)&d, sizeof(d)); + return *this; + } - inline OArchive& operator<<(int i) { stream_.write((char*)&i, sizeof(i)); return *this; } + inline OArchive& operator<<(int i) + { + stream_.write((char*)&i, sizeof(i)); + return *this; + } - inline OArchive& operator<<(std::string s) { stream_.write(s.c_str(), s.size() + 1); return *this; } + inline OArchive& operator<<(std::string s) + { + stream_.write(s.c_str(), s.size() + 1); + return *this; + } inline std::ostream& getStream() { return stream_; } protected: - std::ostream& stream_; - }; -class IArchive { +class IArchive +{ public: - - IArchive(std::istream& stream) : stream_(stream) { - stream_.read((char*)&version_, sizeof(version_)); - } + IArchive(std::istream& stream) : stream_(stream) { stream_.read((char*)&version_, sizeof(version_)); } virtual ~IArchive() {} - inline IArchive& operator>>(float& f) { stream_.read((char*)&f, sizeof(f)); return *this; } + inline IArchive& operator>>(float& f) + { + stream_.read((char*)&f, sizeof(f)); + return *this; + } - inline IArchive& operator>>(double& d) { stream_.read((char*)&d, sizeof(d)); return *this; } + inline IArchive& operator>>(double& d) + { + stream_.read((char*)&d, sizeof(d)); + return *this; + } - inline IArchive& operator>>(int& i) { stream_.read((char*)&i, sizeof(i)); return *this; } + inline IArchive& operator>>(int& i) + { + stream_.read((char*)&i, sizeof(i)); + return *this; + } - inline IArchive& operator>>(std::string& s) { + inline IArchive& operator>>(std::string& s) + { s.clear(); char c; - while(true) { + while (true) + { stream_.read(&c, 1); - if (c == '\0') { + if (c == '\0') + { break; } s += c; @@ -66,13 +91,11 @@ class IArchive { inline int getVersion() { return version_; } protected: - std::istream& stream_; int version_; - }; -} +} // namespace ed #endif diff --git a/include/ed/serialization/serialization.h b/include/ed/serialization/serialization.h index eeccd3b5..0d81e0a2 100644 --- a/include/ed/serialization/serialization.h +++ b/include/ed/serialization/serialization.h @@ -6,12 +6,11 @@ #include -namespace tue { -namespace config { +namespace tue::config +{ class Reader; class Writer; -} -} +} // namespace tue::config namespace ed { @@ -19,29 +18,26 @@ namespace ed class WorldModel; class Entity; class UpdateRequest; -class ConvexHull; +struct ConvexHull; class ImageMask; namespace io { class Reader; class Writer; -} -} +} // namespace io +} // namespace ed namespace ed { // SERIALIZATION -//void serialize(const WorldModel& wm, ed::io::Writer& w, unsigned long since_revision = 0); - - -//void serialize(const Entity& wm, ed::io::Writer& w, unsigned long since_revision = 0); - +// void serialize(const WorldModel& wm, ed::io::Writer& w, unsigned long since_revision = 0); -bool deserialize(io::Reader &r, UpdateRequest& req); +// void serialize(const Entity& wm, ed::io::Writer& w, unsigned long since_revision = 0); +bool deserialize(io::Reader& r, UpdateRequest& req); void serialize(const geo::Pose3D& pose, ed::io::Writer& w); @@ -51,39 +47,30 @@ bool deserialize(tue::config::Reader& r, const std::string& group, geo::Pose3D& bool deserialize(tue::config::Reader& r, const std::string& group, geo::Vec3& p); - void serialize(const ConvexHull& ch, ed::io::Writer& w); bool deserialize(ed::io::Reader& r, ConvexHull& ch); - void serialize(const geo::Shape& s, ed::io::Writer& w); bool deserialize(ed::io::Reader& r, geo::Shape& s); bool deserialize(tue::config::Reader& r, const std::string& group, geo::Shape& s); - void serializeTimestamp(double time, ed::io::Writer& w); bool deserializeTimestamp(ed::io::Reader& r, double& time); - void serialize(const ImageMask& mask, tue::serialization::OutputArchive& m); bool deserialize(tue::serialization::InputArchive& m, ImageMask& mask); - -//void serialize(const WorldModel& wm, tue::config::Writer& w); - - +// void serialize(const WorldModel& wm, tue::config::Writer& w); // DESERIALIZATION +// void deserialize(tue::config::Reader& r, UpdateRequest& req); - -//void deserialize(tue::config::Reader& r, UpdateRequest& req); - -} +} // namespace ed #endif diff --git a/include/ed/server.h b/include/ed/server.h index 7284b4ba..f07a8913 100644 --- a/include/ed/server.h +++ b/include/ed/server.h @@ -6,25 +6,26 @@ #include "ed/property_key_db.h" #include - +#if __has_include() +#include +#else #include +#endif -#include +#include +#include #include +#include + #include "tue/config/configuration.h" #include #include #include -namespace tf2_ros -{ - -class TransformListener; - -} +namespace tf2_ros { class TransformListener; } namespace ed { @@ -33,7 +34,7 @@ class Server { public: - Server(); + explicit Server(const rclcpp::Node::SharedPtr& node); virtual ~Server(); void configure(tue::Configuration& config, bool reconfigure = false); @@ -52,7 +53,7 @@ class Server WorldModelConstPtr world_model() const { - boost::lock_guard lg(mutex_world_); + boost::lock_guard const lg(mutex_world_); return ed::make_shared(*world_model_); } @@ -68,6 +69,8 @@ class Server } private: + //! Shared node handle + rclcpp::Node::SharedPtr node_; mutable boost::mutex mutex_world_; // World model datastructure @@ -95,13 +98,13 @@ class Server //! Profiling diagnostic_updater::Updater updater_; - ros::Publisher pub_stats_; + rclcpp::Publisher::SharedPtr pub_stats_; TFBufferPtr tf_buffer_; TFBufferConstPtr tf_buffer_const_; ed::shared_ptr tf_listener_; }; -} +} // namespace ed #endif diff --git a/include/ed/time.h b/include/ed/time.h index a6e78dae..b99c0f13 100644 --- a/include/ed/time.h +++ b/include/ed/time.h @@ -10,33 +10,34 @@ class Time { public: - Time() : secs_(0) {} Time(double secs) : secs_(secs) {} bool operator<(const Time& rhs) const { return secs_ < rhs.secs_; } - friend std::ostream& operator<< (std::ostream& out, const Time& d) + friend std::ostream& operator<<(std::ostream& out, const Time& d) { - int isecs = d.secs_; + int const isecs = d.secs_; - int h = isecs / 3600; - int m = (isecs / 60) % 60; - int s = isecs % 60; - int ms = 1000 * (d.secs_ - isecs); + int const h = isecs / 3600; + int const m = (isecs / 60) % 60; + int const s = isecs % 60; + int const ms = 1000 * (d.secs_ - isecs); out << h << ":" << m << ":" << s << ":" << ms; return out; } - inline double seconds() const { return secs_; } + [[nodiscard]] + double seconds() const + { + return secs_; + } private: - double secs_; - }; } // end namespace ed diff --git a/include/ed/time_cache.h b/include/ed/time_cache.h index 0cbe6980..6b90bc93 100644 --- a/include/ed/time_cache.h +++ b/include/ed/time_cache.h @@ -7,17 +7,15 @@ namespace ed { -template -class TimeCache +template class TimeCache { public: + using const_iterator = typename std::map::const_iterator; - typedef typename std::map::const_iterator const_iterator; + TimeCache() = default; - TimeCache() : max_size_(0) {} - - ~TimeCache() {} + ~TimeCache() = default; void insert(const Time& t, const T& value) { @@ -49,20 +47,30 @@ class TimeCache } } - inline const_iterator begin() const { return cache_.begin(); } - inline const_iterator end() const { return cache_.end(); } + [[nodiscard]] + const_iterator begin() const + { + return cache_.begin(); + } + [[nodiscard]] + const_iterator end() const + { + return cache_.end(); + } - inline unsigned int size() const { return cache_.size(); } + [[nodiscard]] + unsigned int size() const + { + return cache_.size(); + } void setMaxSize(unsigned int n) { max_size_ = n; } private: - // Cache with items ordered in time std::map cache_; - unsigned int max_size_; - + unsigned int max_size_{0}; }; } // end namespace ed diff --git a/include/ed/types.h b/include/ed/types.h index 18541fe3..608f0bb7 100644 --- a/include/ed/types.h +++ b/include/ed/types.h @@ -1,88 +1,84 @@ #ifndef ED_TYPES_H_ #define ED_TYPES_H_ -//#include -//#include +// #include +// #include #include #include #include #include -namespace tf2_ros { - -class Buffer; - -} +namespace tf2_ros { class Buffer; } namespace ed { -typedef uint64_t Idx; +using Idx = uint64_t; static const Idx INVALID_IDX = std::numeric_limits::max(); // For easy switching to std pointers -using boost::shared_ptr; -using boost::make_shared; using boost::const_pointer_cast; using boost::dynamic_pointer_cast; +using boost::make_shared; +using boost::shared_ptr; using boost::static_pointer_cast; class Measurement; -typedef shared_ptr MeasurementPtr; -typedef shared_ptr MeasurementConstPtr; +using MeasurementPtr = shared_ptr; +using MeasurementConstPtr = shared_ptr; class Entity; -typedef shared_ptr EntityPtr; -typedef shared_ptr EntityConstPtr; +using EntityPtr = shared_ptr; +using EntityConstPtr = shared_ptr; class Plugin; -typedef shared_ptr PluginPtr; -typedef shared_ptr PluginConstPtr; +using PluginPtr = shared_ptr; +using PluginConstPtr = shared_ptr; class WorldModel; -typedef shared_ptr WorldModelPtr; -typedef shared_ptr WorldModelConstPtr; +using WorldModelPtr = shared_ptr; +using WorldModelConstPtr = shared_ptr; class UpdateRequest; -typedef shared_ptr UpdateRequestPtr; -typedef shared_ptr UpdateRequestConstPtr; +using UpdateRequestPtr = shared_ptr; +using UpdateRequestConstPtr = shared_ptr; class PluginContainer; -typedef shared_ptr PluginContainerPtr; -typedef shared_ptr PluginContainerConstPtr; +using PluginContainerPtr = shared_ptr; +using PluginContainerConstPtr = shared_ptr; class SensorModule; -typedef shared_ptr SensorModulePtr; -typedef shared_ptr SensorModuleConstPtr; +using SensorModulePtr = shared_ptr; +using SensorModuleConstPtr = shared_ptr; class RGBDALModule; -typedef shared_ptr RGBDALModulePtr; -typedef shared_ptr RGBDALModuleConstPtr; +using RGBDALModulePtr = shared_ptr; +using RGBDALModuleConstPtr = shared_ptr; class RGBDSegModule; -typedef shared_ptr RGBDSegModulePtr; -typedef shared_ptr RGBDSegModuleConstPtr; +using RGBDSegModulePtr = shared_ptr; +using RGBDSegModuleConstPtr = shared_ptr; class PerceptionModule; -typedef shared_ptr PerceptionModulePtr; -typedef shared_ptr PerceptionModuleConstPtr; +using PerceptionModulePtr = shared_ptr; +using PerceptionModuleConstPtr = shared_ptr; class Relation; -typedef shared_ptr RelationPtr; -typedef shared_ptr RelationConstPtr; +using RelationPtr = shared_ptr; +using RelationConstPtr = shared_ptr; -class ConvexHull2D; +struct ConvexHull2D; class ImageMask; class UUID; -typedef std::string TYPE; +using TYPE = std::string; // tf2_ros::Buffer -typedef shared_ptr TFBufferPtr; -typedef shared_ptr TFBufferConstPtr; +using TFBufferPtr = shared_ptr; +using TFBufferConstPtr = shared_ptr; -} +} // namespace ed #endif diff --git a/include/ed/update_request.h b/include/ed/update_request.h index d23e4b9b..cc963488 100644 --- a/include/ed/update_request.h +++ b/include/ed/update_request.h @@ -1,21 +1,21 @@ #ifndef ED_UPDATE_REQUEST_H_ #define ED_UPDATE_REQUEST_H_ -#include "ed/types.h" -#include "ed/uuid.h" #include "ed/property.h" #include "ed/property_key.h" #include "ed/property_key_db.h" +#include "ed/types.h" +#include "ed/uuid.h" #include +#include #include #include #include -#include -#include "ed/convex_hull_2d.h" #include "ed/convex_hull.h" +#include "ed/convex_hull_2d.h" #include "ed/measurement_convex_hull.h" namespace ed @@ -25,14 +25,16 @@ class UpdateRequest { public: - - UpdateRequest() : is_sync_update(false) {} - + UpdateRequest() = default; // MEASUREMENTS - std::map > measurements; - void addMeasurement(const UUID& id, const MeasurementConstPtr& m) { measurements[id].push_back(m); flagUpdated(id); } + std::map> measurements; + void addMeasurement(const UUID& id, const MeasurementConstPtr& m) + { + measurements[id].push_back(m); + flagUpdated(id); + } void addMeasurements(const UUID& id, const std::vector& measurements_) { @@ -45,24 +47,35 @@ class UpdateRequest flagUpdated(id); } - // VISUALS std::map visuals; [[deprecated("Use setVisual() or setCollision() instead.")]] - void setShape(const UUID& id, const geo::ShapeConstPtr& shape) { setVisual(id, shape); } - void setVisual(const UUID& id, const geo::ShapeConstPtr& visual) { visuals[id] = visual; flagUpdated(id); } + void setShape(const UUID& id, const geo::ShapeConstPtr& shape) + { + setVisual(id, shape); + } + void setVisual(const UUID& id, const geo::ShapeConstPtr& visual) + { + visuals[id] = visual; + flagUpdated(id); + } // COLLISIONS std::map collisions; - void setCollision(const UUID& id, const geo::ShapeConstPtr& collision) { collisions[id] = collision; flagUpdated(id); } + void setCollision(const UUID& id, const geo::ShapeConstPtr& collision) + { + collisions[id] = collision; + flagUpdated(id); + } // VOLUMES - std::map > volumes_added; - void addVolume(const UUID& id, const std::string Volume_name, const geo::ShapeConstPtr& Volume_shape) - { std::map >::iterator it = volumes_added.find(id); + std::map> volumes_added; + void addVolume(const UUID& id, const std::string& Volume_name, const geo::ShapeConstPtr& Volume_shape) + { + auto const it = volumes_added.find(id); if (it != volumes_added.end()) { - std::map::iterator it2 = it->second.find(Volume_name); + auto const it2 = it->second.find(Volume_name); if (it2 != it->second.end()) it2->second = Volume_shape; else @@ -77,14 +90,21 @@ class UpdateRequest flagUpdated(id); } - std::map > volumes_removed; - void removeVolume(const UUID& id, const std::string Volume_name) { volumes_removed[id].insert(Volume_name); flagUpdated(id); } - + std::map> volumes_removed; + void removeVolume(const UUID& id, const std::string& Volume_name) + { + volumes_removed[id].insert(Volume_name); + flagUpdated(id); + } // CONVEX HULLS NEW - std::map > convex_hulls_new; - void setConvexHullNew(const UUID& id, const ed::ConvexHull& convex_hull, const geo::Pose3D& pose, double time, std::string source = "") + std::map> convex_hulls_new; + void setConvexHullNew(const UUID& id, + const ed::ConvexHull& convex_hull, + const geo::Pose3D& pose, + double time, + const std::string& source = "") { ed::MeasurementConvexHull& m = convex_hulls_new[id][source]; m.convex_hull = convex_hull; @@ -99,18 +119,28 @@ class UpdateRequest convex_hulls_new[id][source] = ed::MeasurementConvexHull(); } - // TYPES std::map types; - void setType(const UUID& id, const std::string& type) { types[id] = type; flagUpdated(id); } - - std::map > type_sets_added; - void addType(const UUID& id, const std::string& type) { type_sets_added[id].insert(type); flagUpdated(id); } + void setType(const UUID& id, const std::string& type) + { + types[id] = type; + flagUpdated(id); + } - std::map > type_sets_removed; - void removeType(const UUID& id, const std::string& type) { type_sets_removed[id].insert(type); flagUpdated(id); } + std::map> type_sets_added; + void addType(const UUID& id, const std::string& type) + { + type_sets_added[id].insert(type); + flagUpdated(id); + } + std::map> type_sets_removed; + void removeType(const UUID& id, const std::string& type) + { + type_sets_removed[id].insert(type); + flagUpdated(id); + } // PROBABILITY OF EXISTENCE @@ -118,28 +148,37 @@ class UpdateRequest void setExistenceProbability(const UUID& id, double prob) { existence_probabilities[id] = prob; } - // LAST UPDATE TIMESTAMP std::map last_update_timestamps; void setLastUpdateTimestamp(const UUID& id, double t) { last_update_timestamps[id] = t; } - // POSES std::map poses; - void setPose(const UUID& id, const geo::Pose3D& pose) { poses[id] = pose; flagUpdated(id); } + void setPose(const UUID& id, const geo::Pose3D& pose) + { + poses[id] = pose; + flagUpdated(id); + } std::vector poses_removed; - void removePose(const UUID& id) { poses_removed.push_back(id); flagUpdated(id); } - + void removePose(const UUID& id) + { + poses_removed.push_back(id); + flagUpdated(id); + } // RELATIONS - std::map > relations; - void setRelation(const UUID& id1, const UUID& id2, const RelationConstPtr& r) { relations[id1][id2] = r; flagUpdated(id1); flagUpdated(id2);} - + std::map> relations; + void setRelation(const UUID& id1, const UUID& id2, const RelationConstPtr& r) + { + relations[id1][id2] = r; + flagUpdated(id1); + flagUpdated(id2); + } // DATA @@ -147,7 +186,7 @@ class UpdateRequest void addData(const UUID& id, const tue::config::DataConstPointer& data) { - std::map::iterator it = datas.find(id); + auto const it = datas.find(id); if (it == datas.end()) { datas[id] = data; @@ -164,10 +203,9 @@ class UpdateRequest flagUpdated(id); } - std::map > properties; + std::map> properties; - template - void setProperty(const UUID& id, const PropertyKey& key, const T& value) + template void setProperty(const UUID& id, const PropertyKey& key, const T& value) { if (!key.valid()) return; @@ -186,46 +224,54 @@ class UpdateRequest flagUpdated(id); } - // REMOVED ENTITIES std::set removed_entities; - void removeEntity(const UUID& id) { removed_entities.insert(id); flagUpdated(id); } - + void removeEntity(const UUID& id) + { + removed_entities.insert(id); + flagUpdated(id); + } // FLAGS std::map added_flags; - void setFlag(const UUID& id, const std::string& flag) { added_flags[id] = flag; flagUpdated(id); } + void setFlag(const UUID& id, const std::string& flag) + { + added_flags[id] = flag; + flagUpdated(id); + } std::map removed_flags; - void removeFlag(const UUID& id, const std::string& flag) { removed_flags[id] = flag; flagUpdated(id); } - - + void removeFlag(const UUID& id, const std::string& flag) + { + removed_flags[id] = flag; + flagUpdated(id); + } // UPDATED (AND REMOVED) ENTITIES std::set updated_entities; - bool empty() const { return updated_entities.empty(); } - + [[nodiscard]] + bool empty() const + { + return updated_entities.empty(); + } // Is true if the update was created for synchronization only (used by ed_cloud) - bool is_sync_update; + bool is_sync_update{false}; void setSyncUpdate(bool b = true) { is_sync_update = b; } - private: - void flagUpdated(const ed::UUID& id) { updated_entities.insert(id); } - }; -} +} // namespace ed #endif diff --git a/include/ed/uuid.h b/include/ed/uuid.h index 50ba8af0..b2e37100 100644 --- a/include/ed/uuid.h +++ b/include/ed/uuid.h @@ -3,6 +3,7 @@ #include "ed/types.h" #include +#include namespace ed { @@ -11,35 +12,31 @@ class UUID { public: - UUID() : idx(INVALID_IDX) {} UUID(const char* s) : id_(s), idx(INVALID_IDX) {} - UUID(const std::string& s) : id_(s), idx(INVALID_IDX) {} + UUID(std::string s) : id_(std::move(s)), idx(INVALID_IDX) {} - inline bool operator<(const UUID& rhs) const { return id_ < rhs.id_; } + bool operator<(const UUID& rhs) const { return id_ < rhs.id_; } - inline bool operator==(const UUID& rhs) const { return id_ == rhs.id_; } + bool operator==(const UUID& rhs) const { return id_ == rhs.id_; } - inline bool operator!=(const UUID& rhs) const { return id_ != rhs.id_; } + bool operator!=(const UUID& rhs) const { return id_ != rhs.id_; } - inline const char* c_str() const { return id_.c_str(); } + const char* c_str() const { return id_.c_str(); } - inline const std::string& str() const { return id_; } + const std::string& str() const { return id_; } - friend std::ostream& operator<< (std::ostream& out, const UUID& d) + friend std::ostream& operator<<(std::ostream& out, const UUID& d) { out << d.id_; return out; } private: - std::string id_; public: - mutable Idx idx; - }; } // end namespace ed diff --git a/include/ed/variant.h b/include/ed/variant.h index e0cfe285..69a6ab4d 100644 --- a/include/ed/variant.h +++ b/include/ed/variant.h @@ -10,67 +10,58 @@ namespace ed { -template -struct TypeWrapper +template struct TypeWrapper { - typedef T TYPE; - typedef const T CONSTTYPE; - typedef T& REFTYPE; - typedef const T& CONSTREFTYPE; + using TYPE = T; + using CONSTTYPE = T; + using REFTYPE = T&; + using CONSTREFTYPE = T&; }; -template -struct TypeWrapper +template struct TypeWrapper { - typedef T TYPE; - typedef const T CONSTTYPE; - typedef T& REFTYPE; - typedef const T& CONSTREFTYPE; + using TYPE = T; + using CONSTTYPE = T; + using REFTYPE = T&; + using CONSTREFTYPE = T&; }; -template -struct TypeWrapper +template struct TypeWrapper { - typedef T TYPE; - typedef const T CONSTTYPE; - typedef T& REFTYPE; - typedef const T& CONSTREFTYPE; + using TYPE = T; + using CONSTTYPE = T; + using REFTYPE = T&; + using CONSTREFTYPE = T&; }; -template -struct TypeWrapper +template struct TypeWrapper { - typedef T TYPE; - typedef const T CONSTTYPE; - typedef T& REFTYPE; - typedef const T& CONSTREFTYPE; + using TYPE = T; + using CONSTTYPE = T; + using REFTYPE = T&; + using CONSTREFTYPE = T&; }; class Variant { public: - Variant() { } + Variant() = default; - template - Variant(T inValue) : - mImpl(new VariantImpl::TYPE>(inValue)) - { - } + template Variant(const T& inValue) : mImpl(new VariantImpl::TYPE>(inValue)) {} - template - typename TypeWrapper::REFTYPE getValue() + template typename TypeWrapper::REFTYPE getValue() { - return dynamic_cast::TYPE>&>(*mImpl.get()).mValue; + return dynamic_cast::TYPE>&>(*mImpl).mValue; } - template + template + [[nodiscard]] [[nodiscard]] typename TypeWrapper::CONSTREFTYPE getValue() const { - return dynamic_cast::TYPE>&>(*mImpl.get()).mValue; + return dynamic_cast::TYPE>&>(*mImpl).mValue; } - template - void setValue(typename TypeWrapper::CONSTREFTYPE inValue) + template void setValue(typename TypeWrapper::CONSTREFTYPE inValue) { mImpl.reset(new VariantImpl::TYPE>(inValue)); } @@ -78,15 +69,14 @@ class Variant private: struct AbstractVariantImpl { - virtual ~AbstractVariantImpl() {} + virtual ~AbstractVariantImpl() = default; }; - template - struct VariantImpl : public AbstractVariantImpl + template struct VariantImpl : public AbstractVariantImpl { - VariantImpl(T inValue) : mValue(inValue) { } + VariantImpl(const T& inValue) : mValue(inValue) {} - ~VariantImpl() {} + ~VariantImpl() override = default; T mValue; }; @@ -94,6 +84,6 @@ class Variant boost::shared_ptr mImpl; }; -} // end namespace +} // namespace ed #endif diff --git a/include/ed/world_model.h b/include/ed/world_model.h index 879c50e0..72ec6467 100644 --- a/include/ed/world_model.h +++ b/include/ed/world_model.h @@ -1,8 +1,8 @@ #ifndef ED_WORLD_MODEL_H_ #define ED_WORLD_MODEL_H_ -#include "ed/types.h" #include "ed/time.h" +#include "ed/types.h" #include @@ -14,7 +14,7 @@ namespace ed { class PropertyKeyDB; -class PropertyKeyDBEntry; +struct PropertyKeyDBEntry; // ---------------------------------------------------------------------------------------------------- @@ -22,16 +22,14 @@ class WorldModel { public: - class EntityIterator : public std::iterator { public: - EntityIterator(const std::vector& v) : it_(v.begin()), it_end_(v.end()) { // Skip possible zero-entities (deleted entities) at the beginning - while(it_ != it_end_ && !(*it_)) + while (it_ != it_end_ && !(*it_)) ++it_; } @@ -42,11 +40,21 @@ class WorldModel EntityIterator& operator++() { // Increase iterator and skip possible zero-entities (deleted entities) - do { ++it_; if (it_ == it_end_) break; } while (!(*it_)); + do + { + ++it_; + if (it_ == it_end_) + break; + } while (!(*it_)); return *this; } - EntityIterator operator++(int) { EntityIterator tmp(*this); operator++(); return tmp; } + EntityIterator operator++(int) + { + EntityIterator const tmp(*this); + operator++(); + return tmp; + } bool operator==(const EntityIterator& rhs) { return it_ == rhs.it_; } @@ -55,34 +63,44 @@ class WorldModel const EntityConstPtr& operator*() { return *it_; } private: - std::vector::const_iterator it_; std::vector::const_iterator it_end_; - }; - typedef EntityIterator const_iterator; + using const_iterator = EntityIterator; WorldModel(const PropertyKeyDB* prop_key_db = nullptr); - inline const_iterator begin() const { return const_iterator(entities_); } + [[nodiscard]] + const_iterator begin() const + { + return const_iterator(entities_); + } - inline const_iterator end() const { return const_iterator(entities_.end()); } + [[nodiscard]] + const_iterator end() const + { + return const_iterator(entities_.end()); + } void setEntity(const UUID& id, const EntityConstPtr& e); void removeEntity(const UUID& id); + [[nodiscard]] EntityConstPtr getEntity(const ed::UUID& id) const { - Idx idx; + Idx idx = 0; if (findEntityIdx(id, idx)) return entities_[idx]; - else - return EntityConstPtr(); + return {}; } - size_t numEntities() const { return entity_map_.size(); } + [[nodiscard]] + size_t numEntities() const + { + return entity_map_.size(); + } void update(const UpdateRequest& req); @@ -93,29 +111,61 @@ class WorldModel bool calculateTransform(const UUID& source, const UUID& target, const Time& time, geo::Pose3D& tf) const; /// Warning: the return vector may return null-pointers - const std::vector& entities() const { return entities_; } + [[nodiscard]] + const std::vector& entities() const + { + return entities_; + } /// Warning: the return vector may return null-pointers - const std::vector& relations() const { return relations_; } + [[nodiscard]] + const std::vector& relations() const + { + return relations_; + } - unsigned long revision() const { return revision_; } + [[nodiscard]] + unsigned long revision() const + { + return revision_; + } - const std::vector& entity_revisions() const { return entity_revisions_; } + [[nodiscard]] + const std::vector& entity_revisions() const + { + return entity_revisions_; + } - [[deprecated("Use entity_visual_revisions(), entity_collision_revisions() or entity_volumes_revisions() instead.")]] - const std::vector& entity_shape_revisions() const { return entity_visual_revisions(); } + [[nodiscard]] [[deprecated( + "Use entity_visual_revisions(), entity_collision_revisions() or entity_volumes_revisions() instead.")]] + const std::vector& entity_shape_revisions() const + { + return entity_visual_revisions(); + } - const std::vector& entity_visual_revisions() const { return entity_visual_revisions_; } + [[nodiscard]] + const std::vector& entity_visual_revisions() const + { + return entity_visual_revisions_; + } - const std::vector& entity_collision_revisions() const { return entity_collision_revisions_; } + [[nodiscard]] + const std::vector& entity_collision_revisions() const + { + return entity_collision_revisions_; + } - const std::vector& entity_volumes_revisions() const { return entity_volumes_revisions_; } + [[nodiscard]] + const std::vector& entity_volumes_revisions() const + { + return entity_volumes_revisions_; + } + [[nodiscard]] const PropertyKeyDBEntry* getPropertyInfo(const std::string& name) const; private: - - unsigned long revision_; + unsigned long revision_{0}; std::map entity_map_; @@ -140,10 +190,8 @@ class WorldModel EntityPtr getOrAddEntity(const UUID& id, std::map& new_entities); Idx addNewEntity(const EntityConstPtr& e); - - }; -} +} // namespace ed #endif diff --git a/include/ed/world_model/transform_crawler.h b/include/ed/world_model/transform_crawler.h index cfe178a6..e1f64c84 100644 --- a/include/ed/world_model/transform_crawler.h +++ b/include/ed/world_model/transform_crawler.h @@ -1,16 +1,14 @@ #ifndef ED_WORLD_MODEL_TRANSFORM_CRAWLER_H_ #define ED_WORLD_MODEL_TRANSFORM_CRAWLER_H_ -#include "ed/types.h" #include "ed/time.h" +#include "ed/types.h" #include #include #include -namespace ed -{ -namespace world_model +namespace ed::world_model { /** @@ -25,26 +23,32 @@ class TransformCrawler struct Node { - Node(Idx entity_idx_, const geo::Pose3D& transform_) - : entity_idx(entity_idx_), transform(transform_) {} + Node(Idx entity_idx_, const geo::Pose3D& transform_) : entity_idx(entity_idx_), transform(transform_) {} Idx entity_idx; geo::Pose3D transform; }; public: - TransformCrawler(const WorldModel& wm, const UUID& start_id, const Time& time); bool next(); - bool hasNext() const { return !queue_.empty(); } + [[nodiscard]] + bool hasNext() const + { + return !queue_.empty(); + } - const geo::Pose3D& transform() const { return queue_.front().transform; } + [[nodiscard]] + const geo::Pose3D& transform() const + { + return queue_.front().transform; + } + [[nodiscard]] const EntityConstPtr& entity() const; private: - const WorldModel& wm_; Time time_; @@ -54,11 +58,10 @@ class TransformCrawler std::queue queue_; void pushChildren(const Entity& e, const geo::Pose3D& transform); - }; -} // end namespace world_model +} // namespace ed::world_model -} // end namespace ed +// end namespace ed #endif diff --git a/package.xml b/package.xml index ca27a2ee..1eaf0099 100644 --- a/package.xml +++ b/package.xml @@ -11,41 +11,51 @@ BSD-2-Clause - catkin - - cmake_modules + ament_cmake + ament_index_cpp code_profiler diagnostic_updater - ed_msgs + ed_interfaces geolib2 kdl_parser libopencv-dev libpcl-all-dev orocos_kdl pluginlib + rclcpp rgbd - rgbd_msgs + rgbd_interfaces rosconsole_bridge - roscpp - sdformat + sdformat_vendor + sensor_msgs + std_msgs tf2 tf2_geometry_msgs tf2_ros tinyxml2 tue_config - tue_filesystem tue_serialization - - python3-rospkg - roslib - - catkin_lint_cmake + tue_serialization_interfaces + urdf + + ament_index_python + launch_ros + rclpy + + ament_cmake_clang_format + ament_cmake_clang_tidy + ament_cmake_gtest + ament_cmake_lint_cmake + ament_cmake_xmllint + clang-format-21 + clang-tidy-21 + tue_lint_config doxygen - + ament_cmake diff --git a/plugins/builder_plugin.cpp b/plugins/builder_plugin.cpp index c2a75df0..a0eb2b6d 100644 --- a/plugins/builder_plugin.cpp +++ b/plugins/builder_plugin.cpp @@ -1,42 +1,34 @@ #include "builder_plugin.h" #include -#include -#include #include +#include +#include -#include #include +#include #include // ---------------------------------------------------------------------------------------------------- -BuilderPlugin::BuilderPlugin() -{ -} +BuilderPlugin::BuilderPlugin() {} // ---------------------------------------------------------------------------------------------------- -BuilderPlugin::~BuilderPlugin() -{ -} +BuilderPlugin::~BuilderPlugin() {} // ---------------------------------------------------------------------------------------------------- -void BuilderPlugin::configure(tue::Configuration config) -{ - -} +void BuilderPlugin::configure(tue::Configuration config) {} // ---------------------------------------------------------------------------------------------------- void BuilderPlugin::initialize() { ros::NodeHandle nh; - ros::AdvertiseServiceOptions opt_set_entity = - ros::AdvertiseServiceOptions::create( - "/ed/set_entity", boost::bind(&BuilderPlugin::srvSetEntity, this, _1, _2), ros::VoidPtr(), &cb_queue_); + ros::AdvertiseServiceOptions opt_set_entity = ros::AdvertiseServiceOptions::create( + "/ed/set_entity", boost::bind(&BuilderPlugin::srvSetEntity, this, _1, _2), ros::VoidPtr(), &cb_queue_); srv_set_entity_ = nh.advertiseService(opt_set_entity); } diff --git a/plugins/builder_plugin.h b/plugins/builder_plugin.h index bbfae4fc..da7aa16b 100644 --- a/plugins/builder_plugin.h +++ b/plugins/builder_plugin.h @@ -7,8 +7,8 @@ // Communication #include -#include #include +#include // Configuration #include @@ -20,7 +20,6 @@ class BuilderPlugin : public ed::Plugin { public: - BuilderPlugin(); virtual ~BuilderPlugin(); @@ -32,7 +31,6 @@ class BuilderPlugin : public ed::Plugin void process(const ed::WorldModel& world, ed::UpdateRequest& req); private: - const ed::WorldModel* world_model_; ed::UpdateRequest* update_req_; @@ -43,9 +41,7 @@ class BuilderPlugin : public ed::Plugin ros::ServiceServer srv_set_entity_; - bool srvSetEntity(ed_msgs::SetEntity::Request& req, ed_msgs::SetEntity::Response& res); - }; #endif diff --git a/plugins/gui_plugin.cpp b/plugins/gui_plugin.cpp index cd6683e6..552251e0 100644 --- a/plugins/gui_plugin.cpp +++ b/plugins/gui_plugin.cpp @@ -7,8 +7,8 @@ #include #include -#include #include +#include #include @@ -23,10 +23,9 @@ class ColorRenderResult : public geo::RenderResult { public: - - ColorRenderResult(cv::Mat& image, cv::Mat& z_buffer, const cv::Vec3b& color, float min_depth, float max_depth) - : geo::RenderResult(image.rows, image.rows), image_(image), z_buffer_(z_buffer), color_(color), - min_depth_(min_depth), max_depth_(max_depth) + ColorRenderResult(cv::Mat& image, cv::Mat& z_buffer, const cv::Vec3b& color, float min_depth, float max_depth) : + geo::RenderResult(image.rows, image.rows), image_(image), z_buffer_(z_buffer), color_(color), + min_depth_(min_depth), max_depth_(max_depth) { } @@ -45,13 +44,10 @@ class ColorRenderResult : public geo::RenderResult } protected: - cv::Mat image_; cv::Mat z_buffer_; cv::Vec3b color_; float min_depth_, max_depth_; - - }; // ---------------------------------------------------------------------------------------------------- @@ -65,29 +61,31 @@ bool inPolygon(const std::vector& points_list, const cv::Point2i& p float testy = point.y; unsigned int ii = 0; - for(; ii < points_list.size(); ++ii){ + for (; ii < points_list.size(); ++ii) + { vertx[ii] = points_list[ii].x; verty[ii] = points_list[ii].y; } int i, j, c = 0; - for (i = 0, j = nvert-1; i < nvert; j = i++) { - if ( ((verty[i]>testy) != (verty[j]>testy)) && - (testx < (vertx[j]-vertx[i]) * (testy-verty[i]) / (verty[j]-verty[i]) + vertx[i]) ) - c = !c; + for (i = 0, j = nvert - 1; i < nvert; j = i++) + { + if (((verty[i] > testy) != (verty[j] > testy)) && + (testx < (vertx[j] - vertx[i]) * (testy - verty[i]) / (verty[j] - verty[i]) + vertx[i])) + c = !c; } return c > 0; } // ---------------------------------------------------------------------------------------------------- -int REDS[] = { 255, 0 , 255, 0, 255, 0 , 255}; -int GREENS[] = { 255, 255, 0 , 0, 255, 255, 0 }; -int BLUES[] = { 255, 255, 255, 255, 0, 0, 0 }; +int REDS[] = {255, 0, 255, 0, 255, 0, 255}; +int GREENS[] = {255, 255, 0, 0, 255, 255, 0}; +int BLUES[] = {255, 255, 255, 255, 0, 0, 0}; // ---------------------------------------------------------------------------------------------------- -int hash(const char *str, int max_val) +int hash(const char* str, int max_val) { unsigned long hash = 5381; int c; @@ -126,7 +124,8 @@ bool imageToBinary(const cv::Mat& image, std::vector& data, Image rgb_params[1] = 95; // default is 95 // Compress image - if (!cv::imencode(".jpg", image, data, rgb_params)) { + if (!cv::imencode(".jpg", image, data, rgb_params)) + { std::cout << "RGB image compression failed" << std::endl; return false; } @@ -139,7 +138,8 @@ bool imageToBinary(const cv::Mat& image, std::vector& data, Image params[0] = CV_IMWRITE_PNG_COMPRESSION; params[1] = 1; - if (!cv::imencode(".png", image, data, params)) { + if (!cv::imencode(".png", image, data, params)) + { std::cout << "PNG image compression failed" << std::endl; return false; } @@ -159,9 +159,7 @@ GUIPlugin::GUIPlugin() // ---------------------------------------------------------------------------------------------------- -GUIPlugin::~GUIPlugin() -{ -} +GUIPlugin::~GUIPlugin() {} // ---------------------------------------------------------------------------------------------------- @@ -199,32 +197,32 @@ void GUIPlugin::configure(tue::Configuration config) if (srv_get_measurements_.getService() != srv_get_measurements) { ros::AdvertiseServiceOptions opt_get_measurements = - ros::AdvertiseServiceOptions::create( - srv_get_measurements, boost::bind(&GUIPlugin::srvGetMeasurements, this, _1, _2), ros::VoidPtr(), &cb_queue_); + ros::AdvertiseServiceOptions::create( + srv_get_measurements, + boost::bind(&GUIPlugin::srvGetMeasurements, this, _1, _2), + ros::VoidPtr(), + &cb_queue_); srv_get_measurements_ = nh.advertiseService(opt_get_measurements); } if (srv_set_label_.getService() != srv_set_label) { - ros::AdvertiseServiceOptions opt_set_label = - ros::AdvertiseServiceOptions::create( - srv_set_label, boost::bind(&GUIPlugin::srvSetLabel, this, _1, _2), ros::VoidPtr(), &cb_queue_); + ros::AdvertiseServiceOptions opt_set_label = ros::AdvertiseServiceOptions::create( + srv_set_label, boost::bind(&GUIPlugin::srvSetLabel, this, _1, _2), ros::VoidPtr(), &cb_queue_); srv_set_label_ = nh.advertiseService(opt_set_label); } if (srv_raise_event_.getService() != srv_raise_event) { - ros::AdvertiseServiceOptions opt_raise_event = - ros::AdvertiseServiceOptions::create( - srv_raise_event, boost::bind(&GUIPlugin::srvRaiseEvent, this, _1, _2), ros::VoidPtr(), &cb_queue_); + ros::AdvertiseServiceOptions opt_raise_event = ros::AdvertiseServiceOptions::create( + srv_raise_event, boost::bind(&GUIPlugin::srvRaiseEvent, this, _1, _2), ros::VoidPtr(), &cb_queue_); srv_raise_event_ = nh.advertiseService(opt_raise_event); } if (srv_get_command_.getService() != srv_get_command) { - ros::AdvertiseServiceOptions opt_get_command = - ros::AdvertiseServiceOptions::create( - srv_get_command, boost::bind(&GUIPlugin::srvGetCommand, this, _1, _2), ros::VoidPtr(), &cb_queue_); + ros::AdvertiseServiceOptions opt_get_command = ros::AdvertiseServiceOptions::create( + srv_get_command, boost::bind(&GUIPlugin::srvGetCommand, this, _1, _2), ros::VoidPtr(), &cb_queue_); srv_get_command_ = nh.advertiseService(opt_get_command); } @@ -236,17 +234,19 @@ void GUIPlugin::configure(tue::Configuration config) projector_pose_.setOrigin(geo::Vector3(cam_x, cam_y, cam_z)); projector_pose_.setBasis(geo::Matrix3::identity()); - projector_ = geo::DepthCamera(image_width, image_height, - image_width / (world_width / cam_z), image_height / (world_height / cam_z), - image_width / 2, image_height / 2, - 0, 0); + projector_ = geo::DepthCamera(image_width, + image_height, + image_width / (world_width / cam_z), + image_height / (world_height / cam_z), + image_width / 2, + image_height / 2, + 0, + 0); } // ---------------------------------------------------------------------------------------------------- -void GUIPlugin::initialize() -{ -} +void GUIPlugin::initialize() {} // ---------------------------------------------------------------------------------------------------- @@ -270,7 +270,8 @@ cv::Point2i GUIPlugin::coordinateToPixel(const geo::Vector3& p) const ed::UUID GUIPlugin::getEntityFromClick(const cv::Point2i& p) const { - for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity) + for (ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); + ++it_entity) { const ed::EntityConstPtr& e = *it_entity; if (!e->visual()) @@ -280,7 +281,7 @@ ed::UUID GUIPlugin::getEntityFromClick(const cv::Point2i& p) const if (!chull_points.empty()) { std::vector image_chull(chull_points.size()); - for(unsigned int i = 0; i < chull_points.size(); ++i) + for (unsigned int i = 0; i < chull_points.size(); ++i) image_chull[i] = coordinateToPixel(chull_points[i]); if (inPolygon(image_chull, p)) @@ -303,7 +304,8 @@ void GUIPlugin::publishMapImage() cv::Mat z_buffer(map_image_.rows, map_image_.cols, CV_32FC1, 0.0); - for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity) + for (ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); + ++it_entity) { const ed::EntityConstPtr& e = *it_entity; @@ -323,7 +325,8 @@ void GUIPlugin::publishMapImage() } } - for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity) + for (ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); + ++it_entity) { const ed::EntityConstPtr& e = *it_entity; @@ -338,7 +341,7 @@ void GUIPlugin::publishMapImage() if (!chull_points.empty()) { // Lower polygon - for(unsigned int i = 0; i < chull_points.size(); ++i) + for (unsigned int i = 0; i < chull_points.size(); ++i) { int j = (i + 1) % chull_points.size(); @@ -348,22 +351,23 @@ void GUIPlugin::publishMapImage() cv::line(map_image_, coordinateToPixel(p1.x, p1.y, e->convexHull().min_z), coordinateToPixel(p2.x, p2.y, e->convexHull().min_z), - 0.3 * color, thickness); + 0.3 * color, + thickness); } // Edges in between - for(unsigned int i = 0; i < chull_points.size(); ++i) + for (unsigned int i = 0; i < chull_points.size(); ++i) { const pcl::PointXYZ p = chull_points[i]; cv::line(map_image_, coordinateToPixel(p.x, p.y, e->convexHull().min_z), coordinateToPixel(p.x, p.y, e->convexHull().max_z), - 0.5 * color, thickness); + 0.5 * color, + thickness); } - // Upper polygon - for(unsigned int i = 0; i < chull_points.size(); ++i) + for (unsigned int i = 0; i < chull_points.size(); ++i) { int j = (i + 1) % chull_points.size(); @@ -373,7 +377,8 @@ void GUIPlugin::publishMapImage() cv::line(map_image_, coordinateToPixel(p1.x, p1.y, e->convexHull().max_z), coordinateToPixel(p2.x, p2.y, e->convexHull().max_z), - color, thickness); + color, + thickness); } if (e->type() == "person") @@ -386,10 +391,12 @@ void GUIPlugin::publishMapImage() // Find the global most recent measurement ed::MeasurementConstPtr last_measurement; - for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity) + for (ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); + ++it_entity) { const ed::EntityConstPtr& e = *it_entity; - if (e->lastMeasurement() && (!last_measurement || e->lastMeasurement()->timestamp() > e->lastMeasurement()->timestamp())) + if (e->lastMeasurement() && + (!last_measurement || e->lastMeasurement()->timestamp() > e->lastMeasurement()->timestamp())) last_measurement = e->lastMeasurement(); } @@ -403,7 +410,8 @@ void GUIPlugin::publishMapImage() cv::circle(map_image_, p_2d, 10, cv::Scalar(255, 255, 255)); - rgbd::View view(*last_measurement->image(), 100); // width doesnt matter; we'll go back to world coordinates anyway + rgbd::View view(*last_measurement->image(), + 100); // width doesnt matter; we'll go back to world coordinates anyway geo::Vector3 p1 = view.getRasterizer().project2Dto3D(0, 0) * 3; geo::Vector3 p2 = view.getRasterizer().project2Dto3D(view.getWidth() - 1, 0) * 3; geo::Vector3 p3 = view.getRasterizer().project2Dto3D(0, view.getHeight() - 1) * 3; @@ -439,8 +447,8 @@ void GUIPlugin::publishMapImage() pub_image_map_.publish(msg); } -// cv::imshow("gui", map_image_); -// cv::waitKey(3); + // cv::imshow("gui", map_image_); + // cv::waitKey(3); } // ---------------------------------------------------------------------------------------------------- @@ -470,7 +478,7 @@ bool GUIPlugin::srvGetMeasurements(ed_msgs::GetMeasurements::Request& req, ed_ms cv::Mat rgb_image_masked(rgb_image.rows, rgb_image.cols, CV_8UC3, cv::Scalar(0, 0, 0)); - for(ed::ImageMask::const_iterator it = image_mask.begin(rgb_image.cols); it != image_mask.end(); ++it) + for (ed::ImageMask::const_iterator it = image_mask.begin(rgb_image.cols); it != image_mask.end(); ++it) { cv::Point2i pt = it(); rgb_image_masked.at(pt) = rgb_image.at(pt); @@ -523,8 +531,7 @@ bool GUIPlugin::srvSetLabel(ed_msgs::SetLabel::Request& req, ed_msgs::SetLabel:: res.msg = "[ED] GUI Plugin: srv SetLabel not yet implemented."; std::cout << res.msg << std::endl; - -// std::cout << "Setting entity '" << id << "' to type '" << req.label << "'" << std::endl; + // std::cout << "Setting entity '" << id << "' to type '" << req.label << "'" << std::endl; } return true; @@ -557,7 +564,7 @@ bool parseParam(const std::map& params, const std::str bool GUIPlugin::srvRaiseEvent(ed_msgs::RaiseEvent::Request& req, ed_msgs::RaiseEvent::Response& res) { std::map params; - for(unsigned int i = 0; i < req.param_names.size(); ++i) + for (unsigned int i = 0; i < req.param_names.size(); ++i) params[req.param_names[i]] = req.param_values[i]; if (req.name == "click") @@ -581,9 +588,8 @@ bool GUIPlugin::srvRaiseEvent(ed_msgs::RaiseEvent::Request& req, ed_msgs::RaiseE res.msg = "Deletion not yet implemented"; std::cout << "[ED] GUI Plugin: " << res.msg << std::endl; - -// world_model_->erase(selected_id_); -// res.msg = "Deleted object with id '" + selected_id_ + "'"; + // world_model_->erase(selected_id_); + // res.msg = "Deleted object with id '" + selected_id_ + "'"; } } else if (type == "navigate") @@ -662,7 +668,9 @@ bool GUIPlugin::srvGetCommand(ed_msgs::GetGUICommand::Request& req, ed_msgs::Get res.command = command_; res.command_id = command_id_; - for(std::map::const_iterator it = command_params_.begin(); it != command_params_.end(); ++it) + for (std::map::const_iterator it = command_params_.begin(); + it != command_params_.end(); + ++it) { res.param_names.push_back(it->first); res.param_values.push_back(it->second); @@ -672,5 +680,4 @@ bool GUIPlugin::srvGetCommand(ed_msgs::GetGUICommand::Request& req, ed_msgs::Get return true; } - ED_REGISTER_PLUGIN(GUIPlugin) diff --git a/plugins/gui_plugin.h b/plugins/gui_plugin.h index 4c64a3b9..be002fa5 100644 --- a/plugins/gui_plugin.h +++ b/plugins/gui_plugin.h @@ -7,17 +7,17 @@ // Communication #include -#include #include +#include // Configuration #include // Services +#include #include -#include #include -#include +#include // Map drawing #include @@ -30,7 +30,6 @@ class GUIPlugin : public ed::Plugin { public: - GUIPlugin(); virtual ~GUIPlugin(); @@ -45,7 +44,6 @@ class GUIPlugin : public ed::Plugin const ed::WorldModel* world_model_; - // Map drawing geo::DepthCamera projector_; @@ -76,7 +74,6 @@ class GUIPlugin : public ed::Plugin bool srvGetCommand(ed_msgs::GetGUICommand::Request& req, ed_msgs::GetGUICommand::Response& res); - void handleRequests(); void publishMapImage(); @@ -101,7 +98,6 @@ class GUIPlugin : public ed::Plugin ed::UUID selected_id_; - // HELPER FUNCTIONS ed::UUID getEntityFromClick(const cv::Point2i& p) const; @@ -113,11 +109,7 @@ class GUIPlugin : public ed::Plugin return coordinateToPixel(geo::Vector3(x, y, z)); } - cv::Point2i coordinateToPixel(const pcl::PointXYZ& p) const - { - return coordinateToPixel(p.x, p.y, p.z); - } - + cv::Point2i coordinateToPixel(const pcl::PointXYZ& p) const { return coordinateToPixel(p.x, p.y, p.z); } }; #endif diff --git a/plugins/robot_plugin.cpp b/plugins/robot_plugin.cpp index 36302793..1665b350 100644 --- a/plugins/robot_plugin.cpp +++ b/plugins/robot_plugin.cpp @@ -2,32 +2,33 @@ #include -#include - #include +#include #include #include -#include #include // URDF shape loading -#include -#include +#include #include +#include +#include #include +#include #include // ---------------------------------------------------------------------------------------------------- bool JointRelation::calculateTransform(const ed::Time& t, geo::Pose3D& tf) const { - ed::TimeCache::const_iterator it_low, it_up; + ed::TimeCache::const_iterator it_low; + ed::TimeCache::const_iterator it_up; joint_pos_cache_.getLowerUpper(t, it_low, it_up); - float joint_pos; + float joint_pos = NAN; if (it_low == joint_pos_cache_.end()) { @@ -48,11 +49,11 @@ bool JointRelation::calculateTransform(const ed::Time& t, geo::Pose3D& tf) const else { // Interpolate - float p1 = it_low->second; - float p2 = it_up->second; + float const p1 = it_low->second; + float const p2 = it_up->second; - float dt1 = t.seconds() - it_low->first.seconds(); - float dt2 = it_up->first.seconds() - t.seconds(); + float const dt1 = t.seconds() - it_low->first.seconds(); + float const dt2 = it_up->first.seconds() - t.seconds(); // Linearly interpolate joint positions joint_pos = (p1 * dt2 + p2 * dt1) / (dt1 + dt2); @@ -77,63 +78,66 @@ geo::ShapePtr URDFGeometryToShape(const urdf::GeometrySharedPtr& geom) if (geom->type == urdf::Geometry::MESH) { - urdf::Mesh* mesh = static_cast(geom.get()); + urdf::Mesh const* mesh = static_cast(geom.get()); if (!mesh) { - ROS_WARN_NAMED("RobotPlugin", "[RobotPlugin] Robot model error: No mesh geometry defined"); + RCLCPP_WARN(rclcpp::get_logger("RobotPlugin"), "[RobotPlugin] Robot model error: No mesh geometry defined"); return shape; } - std::string pkg_prefix = "package://"; + std::string const pkg_prefix = "package://"; if (mesh->filename.substr(0, pkg_prefix.size()) == pkg_prefix) { - std::string str = mesh->filename.substr(pkg_prefix.size()); - size_t i_slash = str.find("/"); + std::string const str = mesh->filename.substr(pkg_prefix.size()); + size_t const i_slash = str.find("/"); - std::string pkg = str.substr(0, i_slash); - std::string rel_filename = str.substr(i_slash + 1); - std::string pkg_path = ros::package::getPath(pkg); - std::string abs_filename = pkg_path + "/" + rel_filename; + std::string const pkg = str.substr(0, i_slash); + std::string const rel_filename = str.substr(i_slash + 1); + std::string const pkg_path = ament_index_cpp::get_package_share_directory(pkg); + std::string const abs_filename = pkg_path + "/" + rel_filename; shape = geo::io::readMeshFile(abs_filename, mesh->scale.x); if (!shape) - ROS_ERROR_STREAM_NAMED("RobotPlugin", "[RobotPlugin] Could not load mesh shape from '" << abs_filename << "'"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("RobotPlugin"), + "[RobotPlugin] Could not load mesh shape from '" << abs_filename << "'"); } } else if (geom->type == urdf::Geometry::BOX) { - urdf::Box* box = static_cast(geom.get()); + urdf::Box const* box = static_cast(geom.get()); if (!box) { - ROS_WARN_NAMED("RobotPlugin", "[RobotPlugin] Robot model error: No box geometry defined"); + RCLCPP_WARN(rclcpp::get_logger("RobotPlugin"), "[RobotPlugin] Robot model error: No box geometry defined"); return shape; } - double hx = box->dim.x / 2; - double hy = box->dim.y / 2; - double hz = box->dim.z / 2; + double const hx = box->dim.x / 2; + double const hy = box->dim.y / 2; + double const hz = box->dim.z / 2; shape.reset(new geo::Box(geo::Vector3(-hx, -hy, -hz), geo::Vector3(hx, hy, hz))); } else if (geom->type == urdf::Geometry::CYLINDER) { - urdf::Cylinder* cyl = static_cast(geom.get()); + urdf::Cylinder const* cyl = static_cast(geom.get()); if (!cyl) { - ROS_WARN_NAMED("RobotPlugin", "[RobotPlugin] Robot model error: No cylinder geometry defined"); + RCLCPP_WARN(rclcpp::get_logger("RobotPlugin"), + "[RobotPlugin] Robot model error: No cylinder geometry defined"); return shape; } shape.reset(new geo::Shape()); ed::models::createCylinder(*shape, cyl->radius, cyl->length, 20); } - else if (geom->type == urdf::Geometry::SPHERE) + else if (geom->type == urdf::Geometry::SPHERE) { - urdf::Sphere* sphere = static_cast(geom.get()); + urdf::Sphere const* sphere = static_cast(geom.get()); if (!sphere) { - ROS_WARN_NAMED("RobotPlugin", "[RobotPlugin] Robot model error: No sphere geometry defined"); + RCLCPP_WARN(rclcpp::get_logger("RobotPlugin"), + "[RobotPlugin] Robot model error: No sphere geometry defined"); return shape; } @@ -148,14 +152,18 @@ geo::ShapePtr URDFGeometryToShape(const urdf::GeometrySharedPtr& geom) std::tuple LinkToShapes(const urdf::LinkSharedPtr& link) { - geo::CompositeShapePtr visual, collision; + geo::CompositeShapePtr visual; + geo::CompositeShapePtr collision; - for (urdf::VisualSharedPtr& vis : link->visual_array) + for (urdf::VisualSharedPtr const& vis : link->visual_array) { const urdf::GeometrySharedPtr& geom = vis->geometry; if (!geom) { - ROS_WARN_STREAM_NAMED("RobotPlugin" ,"[RobotPlugin] Robot model error: missing geometry for visual in link: '" << link->name << "'"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("RobotPlugin"), + "[RobotPlugin] Robot model error: missing geometry " + "for visual in link: '" + << link->name << "'"); continue; } @@ -164,7 +172,7 @@ std::tuple LinkToShapes(const urdf::LinkSharedPtr& offset.t = geo::Vector3(o.position.x, o.position.y, o.position.z); offset.R.setRotation(geo::Quaternion(o.rotation.x, o.rotation.y, o.rotation.z, o.rotation.w)); - geo::ShapePtr subshape = URDFGeometryToShape(geom); + geo::ShapePtr const subshape = URDFGeometryToShape(geom); if (!subshape) continue; @@ -173,12 +181,15 @@ std::tuple LinkToShapes(const urdf::LinkSharedPtr& visual->addShape(*subshape, offset); } - for (urdf::CollisionSharedPtr& col : link->collision_array) + for (urdf::CollisionSharedPtr const& col : link->collision_array) { const urdf::GeometrySharedPtr& geom = col->geometry; if (!geom) { - ROS_WARN_STREAM_NAMED("RobotPlugin" ,"[RobotPlugin] Robot model error: missing geometry for collision in link: '" << link->name << "'"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("RobotPlugin"), + "[RobotPlugin] Robot model error: missing geometry " + "for collision in link: '" + << link->name << "'"); continue; } @@ -187,7 +198,7 @@ std::tuple LinkToShapes(const urdf::LinkSharedPtr& offset.t = geo::Vector3(o.position.x, o.position.y, o.position.z); offset.R.setRotation(geo::Quaternion(o.rotation.x, o.rotation.y, o.rotation.z, o.rotation.w)); - geo::ShapePtr subshape = URDFGeometryToShape(geom); + geo::ShapePtr const subshape = URDFGeometryToShape(geom); if (!subshape) continue; @@ -201,19 +212,17 @@ std::tuple LinkToShapes(const urdf::LinkSharedPtr& // ---------------------------------------------------------------------------------------------------- -RobotPlugin::RobotPlugin() : model_initialized_(true) -{ -} +RobotPlugin::RobotPlugin() {} // ---------------------------------------------------------------------------------------------------- -RobotPlugin::~RobotPlugin() -{ -} +RobotPlugin::~RobotPlugin() = default; // ---------------------------------------------------------------------------------------------------- -void RobotPlugin::constructRobot(const ed::UUID& parent_id, const KDL::SegmentMap::const_iterator& it_segment, ed::UpdateRequest& req) +void RobotPlugin::constructRobot(const ed::UUID& parent_id, + const KDL::SegmentMap::const_iterator& it_segment, + ed::UpdateRequest& req) { const KDL::Segment& segment = it_segment->second.segment; @@ -227,7 +236,7 @@ void RobotPlugin::constructRobot(const ed::UUID& parent_id, const KDL::SegmentMa req.setFlag(child_id, "self"); // Create a joint relation and add id - boost::shared_ptr r(new JointRelation(segment)); + boost::shared_ptr const r(new JointRelation(segment)); r->setCacheSize(joint_cache_size_); r->insert(0, 0); req.setRelation(parent_id, child_id, r); @@ -247,29 +256,30 @@ void RobotPlugin::constructRobot(const ed::UUID& parent_id, const KDL::SegmentMa // ---------------------------------------------------------------------------------------------------- -void RobotPlugin::jointCallback(const sensor_msgs::JointState::ConstPtr& msg) +void RobotPlugin::jointCallback(const sensor_msgs::msg::JointState::ConstSharedPtr& msg) { if (msg->name.size() != msg->position.size()) { - ROS_ERROR("[ED RobotPlugin] On joint callback: name and position vector must be of equal length."); + RCLCPP_ERROR(node_->get_logger(), + "[ED RobotPlugin] On joint callback: name and position vector must be of equal length."); return; } - for(unsigned int i = 0; i < msg->name.size(); ++i) + for (unsigned int i = 0; i < msg->name.size(); ++i) { const std::string& name = msg->name[i]; - double pos = msg->position[i]; + double const pos = msg->position[i]; - std::map::iterator it_r = joint_name_to_rel_info_.find(name); + std::map::iterator const it_r = joint_name_to_rel_info_.find(name); if (it_r != joint_name_to_rel_info_.end()) { RelationInfo& info = it_r->second; // Make a copy of the last relation - boost::shared_ptr r(new JointRelation(*info.last_rel)); + boost::shared_ptr const r(new JointRelation(*info.last_rel)); r->setCacheSize(joint_cache_size_); - r->insert(msg->header.stamp.toSec(), pos); + r->insert(rclcpp::Time(msg->header.stamp).seconds(), pos); update_req_->setRelation(info.parent_id, info.child_id, r); @@ -277,7 +287,8 @@ void RobotPlugin::jointCallback(const sensor_msgs::JointState::ConstPtr& msg) } else { - ROS_ERROR_STREAM("[ED RobotPlugin] On joint callback: unknown joint name '" << name << "'."); + RCLCPP_ERROR_STREAM(node_->get_logger(), + "[ED RobotPlugin] On joint callback: unknown joint name '" << name << "'."); } } } @@ -291,30 +302,38 @@ void RobotPlugin::configure(tue::Configuration config) config.value("robot_name", robot_name_); - ros::NodeHandle nh; + cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; if (config.readArray("joint_topics")) { - while(config.nextArrayItem()) + while (config.nextArrayItem()) { std::string topic; config.value("topic", topic); - ROS_DEBUG_STREAM("[RobotPlugin] Topic: " << topic); - - ros::SubscribeOptions sub_options = ros::SubscribeOptions::create - (topic, 10, boost::bind(&RobotPlugin::jointCallback, this, _1), ros::VoidPtr(), &cb_queue_); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "[RobotPlugin] Topic: " << topic); - joint_subscribers_[topic] = nh.subscribe(sub_options); + joint_subscribers_[topic] = node_->create_subscription( + topic, 10, std::bind(&RobotPlugin::jointCallback, this, std::placeholders::_1), sub_options); } config.endArray(); } + executor_.add_callback_group(cb_group_, node_->get_node_base_interface()); + if (config.hasError()) return; + // ToDo(ROS2): in ROS 1 the URDF was fetched from the global parameter server. ROS 2 has no + // global parameter server; this reads it from a parameter on the ED node. Consider subscribing + // to the /robot_description topic (transient_local) instead. std::string urdf_xml; - if (!nh.getParam(urdf_rosparam, urdf_xml)) + if (!node_->has_parameter(urdf_rosparam)) + node_->declare_parameter(urdf_rosparam, ""); + urdf_xml = node_->get_parameter(urdf_rosparam).as_string(); + if (urdf_xml.empty()) { config.addError("No such ROS parameter: '" + urdf_rosparam + "'."); return; @@ -339,9 +358,7 @@ void RobotPlugin::configure(tue::Configuration config) // ---------------------------------------------------------------------------------------------------- -void RobotPlugin::initialize() -{ -} +void RobotPlugin::initialize() {} // ---------------------------------------------------------------------------------------------------- @@ -353,11 +370,12 @@ void RobotPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req) std::vector links; robot_model_.getLinks(links); - for(std::vector::const_iterator it = links.begin(); it != links.end(); ++it) + for (std::vector::const_iterator it = links.begin(); it != links.end(); ++it) { const urdf::LinkSharedPtr& link = *it; - geo::ShapePtr visual, collision; + geo::ShapePtr visual; + geo::ShapePtr collision; std::tie(visual, collision) = LinkToShapes(link); if (visual || collision) { @@ -381,13 +399,13 @@ void RobotPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req) } update_req_ = &req; - cb_queue_.callAvailable(); + executor_.spin_some(); - ed::EntityConstPtr e_robot = world.getEntity(robot_name_); + ed::EntityConstPtr const e_robot = world.getEntity(robot_name_); if (e_robot && e_robot->has_pose()) { // Calculate absolute poses - for(ed::world_model::TransformCrawler tc(world, robot_name_, ros::Time::now().toSec()); tc.hasNext(); tc.next()) + for (ed::world_model::TransformCrawler tc(world, robot_name_, node_->now().seconds()); tc.hasNext(); tc.next()) { const ed::EntityConstPtr& e = tc.entity(); req.setPose(e->id(), e_robot->pose() * tc.transform()); diff --git a/plugins/robot_plugin.h b/plugins/robot_plugin.h index e26822ad..af7fd724 100644 --- a/plugins/robot_plugin.h +++ b/plugins/robot_plugin.h @@ -6,12 +6,11 @@ #include #include -#include -#include -#include +#include +#include -#include #include +#include #include @@ -23,10 +22,9 @@ class JointRelation : public ed::Relation { public: - JointRelation(const KDL::Segment& segment) : segment_(segment) {} - bool calculateTransform(const ed::Time& t, geo::Pose3D& tf) const; + bool calculateTransform(const ed::Time& t, geo::Pose3D& tf) const override; void insert(const ed::Time& t, float joint_pos) { joint_pos_cache_.insert(t, joint_pos); } @@ -35,20 +33,17 @@ class JointRelation : public ed::Relation void setCacheSize(unsigned int n) { joint_pos_cache_.setMaxSize(n); } private: - ed::TimeCache joint_pos_cache_; KDL::Segment segment_; // calculates the joint pose - }; - // ---------------------------------------------------------------------------------------------------- struct RelationInfo { ed::UUID parent_id; ed::UUID child_id; - ed::Idx r_idx; + ed::Idx r_idx{}; boost::shared_ptr last_rel; }; @@ -58,22 +53,20 @@ class RobotPlugin : public ed::Plugin { public: - RobotPlugin(); - virtual ~RobotPlugin(); + ~RobotPlugin() override; - void configure(tue::Configuration config); + void configure(tue::Configuration config) override; - void initialize(); + void initialize() override; - void process(const ed::WorldModel& world, ed::UpdateRequest& req); + void process(const ed::WorldModel& world, ed::UpdateRequest& req) override; private: - std::string robot_name_; - bool model_initialized_; + bool model_initialized_{true}; KDL::Tree tree_; @@ -81,22 +74,23 @@ class RobotPlugin : public ed::Plugin std::map joint_name_to_rel_info_; - ed::UpdateRequest* update_req_; + ed::UpdateRequest* update_req_{}; - unsigned int joint_cache_size_; - - void constructRobot(const ed::UUID& parent_id, const KDL::SegmentMap::const_iterator& it_segment, ed::UpdateRequest& req); + unsigned int joint_cache_size_{}; + void constructRobot(const ed::UUID& parent_id, + const KDL::SegmentMap::const_iterator& it_segment, + ed::UpdateRequest& req); // ROS Communication - ros::CallbackQueue cb_queue_; - - std::map joint_subscribers_; + rclcpp::CallbackGroup::SharedPtr cb_group_; - void jointCallback(const sensor_msgs::JointState::ConstPtr& msg); + rclcpp::executors::SingleThreadedExecutor executor_; + std::map::SharedPtr> joint_subscribers_; + void jointCallback(const sensor_msgs::msg::JointState::ConstSharedPtr& msg); }; #endif diff --git a/plugins/sync_plugin.cpp b/plugins/sync_plugin.cpp index 6679eb31..78e2225c 100644 --- a/plugins/sync_plugin.cpp +++ b/plugins/sync_plugin.cpp @@ -1,24 +1,24 @@ #include "sync_plugin.h" -#include - -#include "ed_msgs/Query.h" -#include "ed/update_request.h" -#include "ed/world_model.h" +#include "ed/init_data.h" +#include "ed/plugin.h" #include "ed/serialization/serialization.h" +#include "ed/update_request.h" #include +#include +#include +#include +#include +#include +#include // ---------------------------------------------------------------------------------------------------- -SyncPlugin::SyncPlugin() : rev_number_(0) -{ -} +SyncPlugin::SyncPlugin() {} // ---------------------------------------------------------------------------------------------------- -SyncPlugin::~SyncPlugin() -{ -} +SyncPlugin::~SyncPlugin() = default; // ---------------------------------------------------------------------------------------------------- @@ -27,45 +27,53 @@ void SyncPlugin::initialize(ed::InitData& init) std::string server_name; init.config.value("server", server_name); - ros::NodeHandle nh; - sync_client_ = nh.serviceClient(server_name); + cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + sync_client_ = node_->create_client(server_name, rclcpp::ServicesQoS(), cb_group_); + executor_.add_callback_group(cb_group_, node_->get_node_base_interface()); } // ---------------------------------------------------------------------------------------------------- void SyncPlugin::process(const ed::PluginInput& /*data*/, ed::UpdateRequest& req) { - ed_msgs::Query query; - query.request.since_revision = rev_number_; + auto request = std::make_shared(); + request->since_revision = rev_number_; - if (!sync_client_.call(query)) + auto future = sync_client_->async_send_request(request); + if (executor_.spin_until_future_complete(future) != rclcpp::FutureReturnCode::SUCCESS) { - ROS_ERROR_STREAM("[ED SyncPlugin] Failed to call service '" << sync_client_.getService() << "'"); + RCLCPP_ERROR_STREAM(node_->get_logger(), + "[ED SyncPlugin] Failed to call service '" << sync_client_->get_service_name() << "'"); return; } - ed::io::JSONReader r(query.response.human_readable.c_str()); + auto response = future.get(); + ed::io::JSONReader r(response->human_readable.c_str()); if (!r.ok()) { - ROS_ERROR_STREAM("[ED SyncPlugin] Could not parse query response received from '" << sync_client_.getService() << "': " << query.response); + RCLCPP_ERROR_STREAM(node_->get_logger(), + "[ED SyncPlugin] Could not parse query response received from '" + << sync_client_->get_service_name() << "'"); return; } -// std::cout << "Response size: " << query.response.human_readable.size() << std::endl; + // std::cout << "Response size: " << response->human_readable.size() << std::endl; ed::deserialize(r, req); if (!r.ok()) { - ROS_ERROR_STREAM("[ED SyncPlugin] Invalid query response from '" << sync_client_.getService() << "': " << r.error()); + RCLCPP_ERROR_STREAM(node_->get_logger(), + "[ED SyncPlugin] Invalid query response from '" << sync_client_->get_service_name() + << "': " << r.error()); // Clear update request req = ed::UpdateRequest(); } else { - rev_number_ = query.response.new_revision; + rev_number_ = response->new_revision; } } diff --git a/plugins/sync_plugin.h b/plugins/sync_plugin.h index 3adf58e9..331c5291 100644 --- a/plugins/sync_plugin.h +++ b/plugins/sync_plugin.h @@ -3,27 +3,29 @@ #include -#include +#include +#include class SyncPlugin : public ed::Plugin { public: - SyncPlugin(); - virtual ~SyncPlugin(); + ~SyncPlugin() override; - void initialize(ed::InitData& init); + void initialize(ed::InitData& init) override; - void process(const ed::PluginInput& data, ed::UpdateRequest& req); + void process(const ed::PluginInput& data, ed::UpdateRequest& req) override; private: + uint64_t rev_number_{0}; - uint64_t rev_number_; + rclcpp::CallbackGroup::SharedPtr cb_group_; - ros::ServiceClient sync_client_; + rclcpp::executors::SingleThreadedExecutor executor_; + rclcpp::Client::SharedPtr sync_client_; }; -#endif //ED_SYNC_PLUGIN_H_ +#endif // ED_SYNC_PLUGIN_H_ diff --git a/plugins/tf_publisher_plugin.cpp b/plugins/tf_publisher_plugin.cpp index 7c1ef688..3148a130 100644 --- a/plugins/tf_publisher_plugin.cpp +++ b/plugins/tf_publisher_plugin.cpp @@ -1,27 +1,30 @@ #include "tf_publisher_plugin.h" +#include "ed/plugin.h" +#include "ed/types.h" -#include #include +#include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include +#include +#include + +#include // ---------------------------------------------------------------------------------------------------- -TFPublisherPlugin::TFPublisherPlugin() : tf_broadcaster_(nullptr) -{ -} +TFPublisherPlugin::TFPublisherPlugin() : tf_broadcaster_(nullptr) {} // ---------------------------------------------------------------------------------------------------- -TFPublisherPlugin::~TFPublisherPlugin() -{ -} +TFPublisherPlugin::~TFPublisherPlugin() = default; // ---------------------------------------------------------------------------------------------------- @@ -40,17 +43,15 @@ void TFPublisherPlugin::configure(tue::Configuration config) void TFPublisherPlugin::initialize() { - tf_broadcaster_ = std::make_unique(); + tf_broadcaster_ = std::make_unique(node_); } // ---------------------------------------------------------------------------------------------------- void TFPublisherPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& /*req*/) { - for(ed::WorldModel::const_iterator it = world.begin(); it != world.end(); ++it) + for (const auto& e : world) { - const ed::EntityConstPtr& e = *it; - if (!e->has_pose()) continue; @@ -64,11 +65,11 @@ void TFPublisherPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& tf2::Stamped t; geo::convert(e->pose(), t); - t.frame_id_ = root_frame_id_; - t.stamp_ = ros::Time::now(); - geometry_msgs::TransformStamped msg; + geometry_msgs::msg::TransformStamped msg; tf2::convert(t, msg); + msg.header.frame_id = root_frame_id_; + msg.header.stamp = node_->now(); msg.child_frame_id = e->id().str(); tf_broadcaster_->sendTransform(msg); } diff --git a/plugins/tf_publisher_plugin.h b/plugins/tf_publisher_plugin.h index efbf5469..82d1b554 100644 --- a/plugins/tf_publisher_plugin.h +++ b/plugins/tf_publisher_plugin.h @@ -5,34 +5,29 @@ #include -namespace tf2_ros { - class TransformBroadcaster; -} +namespace tf2_ros { class TransformBroadcaster; } class TFPublisherPlugin : public ed::Plugin { public: - TFPublisherPlugin(); - virtual ~TFPublisherPlugin(); + ~TFPublisherPlugin() override; - void configure(tue::Configuration config); + void configure(tue::Configuration config) override; - void initialize(); + void initialize() override; - void process(const ed::WorldModel& world, ed::UpdateRequest& req); + void process(const ed::WorldModel& world, ed::UpdateRequest& req) override; private: - std::string root_frame_id_; // Exclude all ids starting with this value std::string exclude_; std::unique_ptr tf_broadcaster_; - }; #endif diff --git a/src/convex_hull_2d.cpp b/src/convex_hull_2d.cpp index 2abc2161..f779a031 100644 --- a/src/convex_hull_2d.cpp +++ b/src/convex_hull_2d.cpp @@ -1,4 +1,8 @@ +#include + #include "ed/convex_hull_2d.h" +#include +#include namespace ed { @@ -6,22 +10,23 @@ namespace ed double ConvexHull2D::area() const { double a = 0.0; - for ( pcl::PointCloud::const_iterator ch_it = chull.begin(); ch_it != chull.end(); ++ch_it) + for (auto ch_it = chull.begin(); ch_it != chull.end(); ++ch_it) { - double x1 = ch_it->x; - double y1 = ch_it->y; - double x2; - double y2; - if (ch_it != chull.end()-1) + double const x1 = ch_it->x; + double const y1 = ch_it->y; + double x2 = NAN; + double y2 = NAN; + if (ch_it != chull.end() - 1) { - x2 = (ch_it+1)->x; - y2 = (ch_it+1)->y; - } else + x2 = (ch_it + 1)->x; + y2 = (ch_it + 1)->y; + } + else { x2 = chull.begin()->x; y2 = chull.begin()->y; } - a = a + 0.5*(x1*y2 - x2*y1); + a = a + (0.5 * (x1 * y2 - x2 * y1)); } return a; } @@ -36,4 +41,4 @@ double ConvexHull2D::volume() const return area() * height(); } -} +} // namespace ed diff --git a/src/convex_hull_calc.cpp b/src/convex_hull_calc.cpp index 19e886e3..1a582d57 100644 --- a/src/convex_hull_calc.cpp +++ b/src/convex_hull_calc.cpp @@ -1,11 +1,16 @@ #include "ed/convex_hull_calc.h" - +#include "ed/convex_hull.h" + +#include +#include +#include +#include +#include +#include #include +#include -namespace ed -{ - -namespace convex_hull +namespace ed::convex_hull { // ---------------------------------------------------------------------------------------------------- @@ -21,7 +26,7 @@ namespace convex_hull void create(const std::vector& points, float z_min, float z_max, ConvexHull& chull, geo::Pose3D& pose) { cv::Mat_ points_2d(1, points.size()); - for(unsigned int i = 0; i < points.size(); ++i) + for (unsigned int i = 0; i < points.size(); ++i) points_2d.at(i) = cv::Vec2f(points[i].x, points[i].y); pose = geo::Pose3D::identity(); @@ -38,10 +43,10 @@ void create(const std::vector& points, float z_min, float z_max, Con geo::Vec2f xy_max(-1e9, -1e9); chull.points.clear(); - for(unsigned int i = 0; i < chull_indices.size(); ++i) + for (int const chull_indice : chull_indices) { - const cv::Vec2f& p_cv = points_2d.at(chull_indices[i]); - geo::Vec2f p(p_cv[0], p_cv[1]); + const cv::Vec2f& p_cv = points_2d.at(chull_indice); + geo::Vec2f const p(p_cv[0], p_cv[1]); chull.points.push_back(p); @@ -57,9 +62,8 @@ void create(const std::vector& points, float z_min, float z_max, Con pose.t.y = (xy_min.y + xy_max.y) / 2; // Move all points to the pose frame - for(unsigned int i = 0; i < chull.points.size(); ++i) + for (auto& p : chull.points) { - geo::Vec2f& p = chull.points[i]; p.x -= pose.t.x; p.y -= pose.t.y; } @@ -83,7 +87,7 @@ void create(const std::vector& points, float z_min, float z_max, Con void createAbsolute(const std::vector& points, float z_min, float z_max, ConvexHull& chull) { cv::Mat_ points_2d(1, points.size()); - for(unsigned int i = 0; i < points.size(); ++i) + for (unsigned int i = 0; i < points.size(); ++i) points_2d.at(i) = cv::Vec2f(points[i].x, points[i].y); chull.z_min = z_min; @@ -93,7 +97,7 @@ void createAbsolute(const std::vector& points, float z_min, float z_ cv::convexHull(points_2d, chull_indices); chull.points.resize(chull_indices.size()); - for(unsigned int i = 0; i < chull_indices.size(); ++i) + for (unsigned int i = 0; i < chull_indices.size(); ++i) { const cv::Vec2f& p_cv = points_2d.at(chull_indices[i]); chull.points[i] = geo::Vec2f(p_cv[0], p_cv[1]); @@ -113,15 +117,15 @@ void calculateEdgesAndNormals(ConvexHull& chull) chull.edges.resize(chull.points.size()); chull.normals.resize(chull.points.size()); - for(unsigned int i = 0; i < chull.points.size(); ++i) + for (unsigned int i = 0; i < chull.points.size(); ++i) { - unsigned int j = (i + 1) % chull.points.size(); + unsigned int const j = (i + 1) % chull.points.size(); const geo::Vec2f& p1 = chull.points[i]; const geo::Vec2f& p2 = chull.points[j]; // Calculate edge - geo::Vec2f e = p2 - p1; + geo::Vec2f const e = p2 - p1; chull.edges[i] = e; // Calculate normal @@ -141,21 +145,24 @@ void calculateEdgesAndNormals(ConvexHull& chull) * @param z_padding padding in z-plane * @return */ -bool collide(const ConvexHull& c1, const geo::Vector3& pos1, - const ConvexHull& c2, const geo::Vector3& pos2, - float xy_padding, float z_padding) +bool collide(const ConvexHull& c1, + const geo::Vector3& pos1, + const ConvexHull& c2, + const geo::Vector3& pos2, + float xy_padding, + float z_padding) { if (c1.points.size() < 3 || c2.points.size() < 3) return false; - float z_diff = pos2.z - pos1.z; + float const z_diff = pos2.z - pos1.z; - if (c1.z_max < (c2.z_min + z_diff - 2 * z_padding) || c2.z_max < (c1.z_min - z_diff - 2 * z_padding)) + if (c1.z_max < (c2.z_min + z_diff - (2 * z_padding)) || c2.z_max < (c1.z_min - z_diff - (2 * z_padding))) return false; - geo::Vec2f pos_diff(pos2.x - pos1.x, pos2.y - pos1.y); + geo::Vec2f const pos_diff(pos2.x - pos1.x, pos2.y - pos1.y); - for(unsigned int i = 0; i < c1.points.size(); ++i) + for (unsigned int i = 0; i < c1.points.size(); ++i) { const geo::Vec2f& p1 = c1.points[i]; const geo::Vec2f& n = c1.normals[i]; @@ -163,10 +170,10 @@ bool collide(const ConvexHull& c1, const geo::Vector3& pos1, // Calculate min and max projection of c1 float min1 = n.dot(c1.points[0] - p1); float max1 = min1; - for(unsigned int k = 1; k < c1.points.size(); ++k) + for (unsigned int k = 1; k < c1.points.size(); ++k) { // Calculate projection - float p = n.dot(c1.points[k] - p1); + float const p = n.dot(c1.points[k] - p1); min1 = std::min(min1, p); max1 = std::max(max1, p); } @@ -176,7 +183,7 @@ bool collide(const ConvexHull& c1, const geo::Vector3& pos1, max1 += xy_padding; // Calculate p1 in c2's frame - geo::Vec2f p1_c2 = p1 - pos_diff; + geo::Vec2f const p1_c2 = p1 - pos_diff; // If this bool stays true, there is definitely no collision bool no_collision = true; @@ -188,10 +195,10 @@ bool collide(const ConvexHull& c1, const geo::Vector3& pos1, bool above = false; // Check if c2's points overlap with c1's bounds - for(unsigned int k = 0; k < c2.points.size(); ++k) + for (const auto& point : c2.points) { // Calculate projection on p1's normal - float p = n.dot(c2.points[k] - p1_c2); + float const p = n.dot(point - p1_c2); below = below || (p < max1); above = above || (p > min1); @@ -221,9 +228,9 @@ bool collide(const ConvexHull& c1, const geo::Vector3& pos1, void calculateArea(ConvexHull& c) { c.area = 0; - for(unsigned int i = 0; i < c.points.size(); ++i) + for (unsigned int i = 0; i < c.points.size(); ++i) { - unsigned int j = (i + 1) % c.points.size(); + unsigned int const j = (i + 1) % c.points.size(); const geo::Vec2f& p1 = c.points[i]; const geo::Vec2f& p2 = c.points[j]; @@ -234,6 +241,4 @@ void calculateArea(ConvexHull& c) // ---------------------------------------------------------------------------------------------------- -} - -} +} // namespace ed::convex_hull diff --git a/src/ed.cpp b/src/ed.cpp index 525892f8..b236296b 100644 --- a/src/ed.cpp +++ b/src/ed.cpp @@ -1,93 +1,104 @@ -#include -#include -#include -#include -#include -#include - +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ed/property.h" #include "ed/server.h" #include -#include // Query #include -#include #include -#include +#include +#include #include +#include +#include +#include +#include +#include #include -#include -#include #include "ed/io/json_writer.h" +#include // Update -#include #include "ed/io/json_reader.h" +#include "ed/types.h" #include "ed/update_request.h" +#include // Reset -#include +#include // Configure -#include +#include // Loop #include // Plugin loading -#include -#include #include -#include -#include -#include +#include "ed/error_context.h" +#include "ed/variant.h" +#include +#include +#include #include -#include -#include +#include #include -#include -#include "ed/error_context.h" -#include +#include -boost::thread::id main_thread_id; +static boost::thread::id main_thread_id; -ed::Server* ed_wm; -std::string update_request_; +static ed::Server* ed_wm; +static std::string update_request_; // ---------------------------------------------------------------------------------------------------- -bool srvReset(ed_msgs::Reset::Request& req, ed_msgs::Reset::Response& /*res*/) +static void srvReset(const std::shared_ptr& req, + const std::shared_ptr& /*res*/) { - ed_wm->reset(req.keep_all_shapes); - return true; + ed_wm->reset(req->keep_all_shapes); } // ---------------------------------------------------------------------------------------------------- -bool srvUpdate(ed_msgs::UpdateSrv::Request& req, ed_msgs::UpdateSrv::Response& res) +static void srvUpdate(const std::shared_ptr& req, + const std::shared_ptr& res) { - ed::io::JSONReader r(req.request.c_str()); + ed::io::JSONReader r(req->request.c_str()); if (!r.ok()) { - res.response = r.error(); - return true; + res->response = r.error(); + return; } ed::UpdateRequest update_req; if (r.readArray("entities")) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { std::string id; if (!r.readValue("id", id)) { - res.response += "Entities should have field 'id'.\n"; + res->response += "Entities should have field 'id'.\n"; continue; } @@ -97,7 +108,7 @@ bool srvUpdate(ed_msgs::UpdateSrv::Request& req, ed_msgs::UpdateSrv::Response& r if (action == "remove") update_req.removeEntity(id); else - res.response += "Unknown action '" + action + "'.\n"; + res->response += "Unknown action '" + action + "'.\n"; } std::string type; @@ -108,26 +119,30 @@ bool srvUpdate(ed_msgs::UpdateSrv::Request& req, ed_msgs::UpdateSrv::Response& r if (r.readGroup("pose")) { - double x, y, z; + double x = NAN; + double y = NAN; + double z = NAN; std::string remove; if (r.readValue("x", x) && r.readValue("y", y) && r.readValue("z", z)) { geo::Pose3D pose = geo::Pose3D::identity(); pose.t = geo::Vector3(x, y, z); - double X, Y, Z; + double X = NAN; + double Y = NAN; + double Z = NAN; if (r.readValue("X", X) && r.readValue("Y", Y) && r.readValue("Z", Z)) pose.setRPY(X, Y, Z); update_req.setPose(id, pose); } - else if(r.readValue("remove", remove) && remove == "true") + else if (r.readValue("remove", remove) && remove == "true") { update_req.removePose(id); } else { - res.response += "For entity '" + id + "': invalid pose (position).\n"; + res->response += "For entity '" + id + "': invalid pose (position).\n"; } r.endGroup(); @@ -135,7 +150,7 @@ bool srvUpdate(ed_msgs::UpdateSrv::Request& req, ed_msgs::UpdateSrv::Response& r if (r.readArray("flags")) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { std::string flag; if (r.readValue("add", flag)) @@ -143,7 +158,7 @@ bool srvUpdate(ed_msgs::UpdateSrv::Request& req, ed_msgs::UpdateSrv::Response& r else if (r.readValue("remove", flag)) update_req.removeFlag(id, flag); else - res.response += "For entity '" + id + "': flag list should only contain 'add' or 'remove'.\n"; + res->response += "For entity '" + id + "': flag list should only contain 'add' or 'remove'.\n"; } } @@ -152,16 +167,16 @@ bool srvUpdate(ed_msgs::UpdateSrv::Request& req, ed_msgs::UpdateSrv::Response& r std::string data_str; if (r.readValue("data", data_str)) { - tue::Configuration data_config; - if (tue::config::loadFromYAMLString(data_str, data_config)) - { - update_req.addData(id, data_config.data()); - } + tue::Configuration data_config; + if (tue::config::loadFromYAMLString(data_str, data_config)) + { + update_req.addData(id, data_config.data()); + } } if (r.readArray("properties")) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { std::string prop_name; if (!r.readValue("name", prop_name)) @@ -171,13 +186,13 @@ bool srvUpdate(ed_msgs::UpdateSrv::Request& req, ed_msgs::UpdateSrv::Response& r const ed::PropertyKeyDBEntry* entry = ed_wm->getPropertyKeyDBEntry(prop_name); if (!entry) { - res.response += "For entity '" + id + "': unknown property '" + prop_name +"'.\n"; + res->response += "For entity '" + id + "': unknown property '" + prop_name + "'.\n"; continue; } if (!entry->info->serializable()) { - res.response += "For entity '" + id + "': property '" + prop_name +"' is not serializable.\n"; + res->response += "For entity '" + id + "': property '" + prop_name + "' is not serializable.\n"; continue; } @@ -185,7 +200,8 @@ bool srvUpdate(ed_msgs::UpdateSrv::Request& req, ed_msgs::UpdateSrv::Response& r if (entry->info->deserialize(r, value)) update_req.setProperty(id, entry, value); else - res.response += "For entity '" + id + "': deserialization of property '" + prop_name +"' failed.\n"; + res->response += + "For entity '" + id + "': deserialization of property '" + prop_name + "' failed.\n"; } r.endArray(); @@ -204,33 +220,32 @@ bool srvUpdate(ed_msgs::UpdateSrv::Request& req, ed_msgs::UpdateSrv::Response& r } else { - res.response += r.error(); + res->response += r.error(); } - - return true; } // ---------------------------------------------------------------------------------------------------- -bool srvQuery(ed_msgs::Query::Request& req, ed_msgs::Query::Response& res) +static void srvQuery(const std::shared_ptr& req, + const std::shared_ptr& res) { // Set of queried ids - std::set ids(req.ids.begin(), req.ids.end()); + std::set ids(req->ids.begin(), req->ids.end()); // convert property names to indexes std::vector property_idxs; - for(std::vector::const_iterator it = req.properties.begin(); it != req.properties.end(); ++it) + for (const auto& propertie : req->properties) { // ToDo: is this thread safe? - const ed::PropertyKeyDBEntry* entry = ed_wm->getPropertyKeyDBEntry(*it); + const ed::PropertyKeyDBEntry* entry = ed_wm->getPropertyKeyDBEntry(propertie); if (entry) property_idxs.push_back(entry->idx); } // Make a copy of the WM, to keep it thead safe - ed::WorldModel wm = *ed_wm->world_model(); + ed::WorldModel const wm = *ed_wm->world_model(); const std::vector& entity_revs = wm.entity_revisions(); - const std::vector& entities = wm.entities(); + const std::vector& entities = wm.entities(); std::vector removed_entities; @@ -239,9 +254,9 @@ bool srvQuery(ed_msgs::Query::Request& req, ed_msgs::Query::Response& res) w.writeArray("entities"); - for(ed::Idx i = 0; i < entity_revs.size(); ++i) + for (ed::Idx i = 0; i < entity_revs.size(); ++i) { - if (req.since_revision >= entity_revs[i]) + if (req->since_revision >= entity_revs[i]) continue; const ed::EntityConstPtr& e = entities[i]; @@ -255,7 +270,7 @@ bool srvQuery(ed_msgs::Query::Request& req, ed_msgs::Query::Response& res) { w.addArrayItem(); w.writeValue("id", e->id().str()); - w.writeValue("idx", (int) i); + w.writeValue("idx", static_cast(i)); // Write type w.writeValue("type", e->type()); @@ -269,7 +284,7 @@ bool srvQuery(ed_msgs::Query::Request& req, ed_msgs::Query::Response& res) } // Write convex hull - if (!e->convexHull().points.empty() && wm.entity_visual_revisions()[i] > req.since_revision) + if (!e->convexHull().points.empty() && wm.entity_visual_revisions()[i] > req->since_revision) { w.writeGroup("convex_hull"); ed::serialize(e->convexHull(), w); @@ -285,7 +300,7 @@ bool srvQuery(ed_msgs::Query::Request& req, ed_msgs::Query::Response& res) } // Mesh - if (e->visual() && wm.entity_visual_revisions()[i] > req.since_revision) + if (e->visual() && wm.entity_visual_revisions()[i] > req->since_revision) { w.writeGroup("mesh"); ed::serialize(*e->visual(), w); @@ -311,12 +326,12 @@ bool srvQuery(ed_msgs::Query::Request& req, ed_msgs::Query::Response& res) const std::map& properties = e->properties(); - if (req.properties.empty()) + if (req->properties.empty()) { - for(std::map::const_iterator it = properties.begin(); it != properties.end(); ++it) + for (const auto& propertie : properties) { - const ed::Property& prop = it->second; - if (req.since_revision < prop.revision && prop.entry->info->serializable()) + const ed::Property& prop = propertie.second; + if (req->since_revision < prop.revision && prop.entry->info->serializable()) { w.addArrayItem(); w.writeValue("name", prop.entry->name); @@ -327,13 +342,13 @@ bool srvQuery(ed_msgs::Query::Request& req, ed_msgs::Query::Response& res) } else { - for(std::vector::const_iterator it = property_idxs.begin(); it != property_idxs.end(); ++it) + for (unsigned long const property_idx : property_idxs) { - std::map::const_iterator it_prop = properties.find(*it); + auto const it_prop = properties.find(property_idx); if (it_prop != properties.end()) { const ed::Property& prop = it_prop->second; - if (req.since_revision < prop.revision && prop.entry->info->serializable()) + if (req->since_revision < prop.revision && prop.entry->info->serializable()) { w.addArrayItem(); w.writeValue("name", prop.entry->name); @@ -358,45 +373,43 @@ bool srvQuery(ed_msgs::Query::Request& req, ed_msgs::Query::Response& res) w.endArray(); if (!removed_entities.empty()) - w.writeValue("removed_entities", &removed_entities[0], removed_entities.size()); + w.writeValue("removed_entities", removed_entities.data(), removed_entities.size()); w.finish(); - res.human_readable = out.str(); - res.new_revision = wm.revision(); - - return true; + res->human_readable = out.str(); + res->new_revision = wm.revision(); } // ---------------------------------------------------------------------------------------------------- -bool srvSimpleQuery(ed_msgs::SimpleQuery::Request& req, ed_msgs::SimpleQuery::Response& res) +static void srvSimpleQuery(const std::shared_ptr& req, + const std::shared_ptr& res) { - double radius = req.radius; + double const radius = req->radius; geo::Vector3 center_point; - geo::convert(req.center_point, center_point); + geo::convert(req->center_point, center_point); // Make a copy of the WM, to keep it thead safe - ed::WorldModel wm = *ed_wm->world_model(); - for(ed::WorldModel::const_iterator it = wm.begin(); it != wm.end(); ++it) + ed::WorldModel const wm = *ed_wm->world_model(); + for (const auto& e : wm) { - const ed::EntityConstPtr& e = *it; - if (!req.id.empty() && e->id() != ed::UUID(req.id)) + if (!req->id.empty() && e->id() != ed::UUID(req->id)) continue; if (!e->has_pose()) continue; - if (!req.type.empty()) + if (!req->type.empty()) { - if (req.type == "unknown") + if (req->type == "unknown") { - if (e->type() != "") + if (!e->type().empty()) continue; } else { - if (!e->hasType(req.type)) + if (!e->hasType(req->type)) continue; } } @@ -405,13 +418,14 @@ bool srvSimpleQuery(ed_msgs::SimpleQuery::Request& req, ed_msgs::SimpleQuery::Re { bool geom_ok = false; - if(req.ignore_z) + if (req->ignore_z) center_point.z = e->pose().t.z; // Ignoring z in global frame, not in entity frame, as it can be rotated - geo::ShapeConstPtr visual = e->visual(); + geo::ShapeConstPtr const visual = e->visual(); if (visual) { - geo::Vector3 center_point_e = e->pose().getBasis().transpose() * (center_point - e->pose().getOrigin()); + geo::Vector3 const center_point_e = + e->pose().getBasis().transpose() * (center_point - e->pose().getOrigin()); if (radius > 0) geom_ok = visual->intersect(center_point_e, radius); else @@ -426,23 +440,21 @@ bool srvSimpleQuery(ed_msgs::SimpleQuery::Request& req, ed_msgs::SimpleQuery::Re continue; } - res.entities.push_back(ed_msgs::EntityInfo()); - convert(*e, res.entities.back()); - + res->entities.emplace_back(); + convert(*e, res->entities.back()); } - - return true; } // ---------------------------------------------------------------------------------------------------- -bool srvConfigure(ed_msgs::Configure::Request& req, ed_msgs::Configure::Response& res) +static void srvConfigure(const std::shared_ptr& req, + const std::shared_ptr& res) { tue::Configuration config; - if (!tue::config::loadFromYAMLString(req.request, config)) + if (!tue::config::loadFromYAMLString(req->request, config)) { - res.error_msg = config.error(); - return true; + res->error_msg = config.error(); + return; } // Configure ED @@ -450,35 +462,21 @@ bool srvConfigure(ed_msgs::Configure::Request& req, ed_msgs::Configure::Response if (config.hasError()) { - res.error_msg = config.error(); - return true; + res->error_msg = config.error(); + return; } - - return true; } // ---------------------------------------------------------------------------------------------------- -bool getEnvironmentVariable(const std::string& var, std::string& value) -{ - const char * val = ::getenv(var.c_str()); - if ( val == nullptr ) - return false; - - value = val; - return true; -} - -// ---------------------------------------------------------------------------------------------------- - -void signalHandler( int sig ) +static void signalHandler(int sig) { // Make sure to remove all signal handlers signal(SIGSEGV, SIG_DFL); signal(SIGABRT, SIG_DFL); std::cerr << "\033[38;5;1m"; - std::cerr << "[ED] ED Crashed!" << std::endl << std::endl; + std::cerr << "[ED] ED Crashed!" << '\n' << '\n'; // - - - - - - - - - - - - - - - - - - - - - - - - - - - - // Print signal @@ -490,7 +488,7 @@ void signalHandler( int sig ) std::cerr << "abort"; else std::cerr << "unknown"; - std::cerr << std::endl << std::endl; + std::cerr << '\n' << '\n'; // - - - - - - - - - - - - - - - - - - - - - - - - - - - - // Print thread name @@ -498,7 +496,7 @@ void signalHandler( int sig ) std::cerr << " Thread: "; char name[1000]; - size_t name_size = 1000; + size_t const name_size = 1000; if (pthread_getname_np(pthread_self(), name, name_size) == 0) { if (std::string(name) == "ed_main") @@ -514,7 +512,7 @@ void signalHandler( int sig ) else std::cerr << "name unknown (id = " << boost::this_thread::get_id() << ")"; - std::cerr << std::endl << std::endl; + std::cerr << '\n' << '\n'; // - - - - - - - - - - - - - - - - - - - - - - - - - - - - // Print error context @@ -524,42 +522,42 @@ void signalHandler( int sig ) ed::ErrorContextData* edata = ed::ErrorContext::data(); if (edata && !edata->stack.empty()) { - std::cerr << std::endl << std::endl; + std::cerr << '\n' << '\n'; - for(unsigned int i = edata->stack.size(); i > 0; --i) + for (unsigned int i = edata->stack.size(); i > 0; --i) { - const char* message = edata->stack[i-1].first; - const char* value = edata->stack[i-1].second; + const char* message = edata->stack[i - 1].first; + const char* value = edata->stack[i - 1].second; if (message) { std::cerr << " " << message; if (value) std::cerr << " " << value; - std::cerr << std::endl; + std::cerr << '\n'; } } } else std::cerr << "unknown"; - std::cerr << std::endl << std::endl; + std::cerr << '\n' << '\n'; // - - - - - - - - - - - - - - - - - - - - - - - - - - - - // Print backtrace - std::cerr << "--------------------------------------------------" << std::endl; - std::cerr << "Backtrace: " << std::endl << std::endl; + std::cerr << "--------------------------------------------------" << '\n'; + std::cerr << "Backtrace: " << '\n' << '\n'; - void *array[20]; - size_t size; + void* array[20]; + size_t size = 0; // get void*'s for all entries on the stack size = backtrace(array, 20); // print out all the frames to stderr backtrace_symbols_fd(array, size, STDERR_FILENO); - std::cerr << "\033[0m" << std::endl; + std::cerr << "\033[0m" << '\n'; exit(1); } @@ -567,7 +565,8 @@ void signalHandler( int sig ) int main(int argc, char** argv) { - ros::init(argc, argv, "ed"); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr const node = rclcpp::Node::make_shared("ed"); // Set the name of the main thread pthread_setname_np(pthread_self(), "ed_main"); @@ -579,22 +578,22 @@ int main(int argc, char** argv) signal(SIGSEGV, signalHandler); signal(SIGABRT, signalHandler); - ed::ErrorContext errc("Start ED server", "init"); + ed::ErrorContext const errc("Start ED server", "init"); // Create the ED server - ed::Server server; + ed::Server server(node); ed_wm = &server; // - - - - - - - - - - - - - - - configure - - - - - - - - - - - - - - - - errc.change("Start ED server", "configure"); + ed::ErrorContext::change("Start ED server", "configure"); tue::Configuration config; // Check if a config file was provided. If so, load it. If not, load the default AMIGO config. if (argc >= 2) { - std::string yaml_filename = argv[1]; + std::string const yaml_filename = argv[1]; config.loadFromYAMLFile(yaml_filename); // Configure ED @@ -602,40 +601,38 @@ int main(int argc, char** argv) if (config.hasError()) { - ROS_ERROR_STREAM(std::endl << "Error during configuration:" << std::endl << std::endl << config.error()); + RCLCPP_ERROR_STREAM(node->get_logger(), + '\n' << "Error during configuration:" << '\n' + << '\n' + << config.error()); return 1; } } // - - - - - - - - - - - - service initialization - - - - - - - - - - - - - errc.change("Start ED server", "service init"); - - ros::NodeHandle nh; - ros::NodeHandle nh_private("~"); - - ros::CallbackQueue cb_queue; + ed::ErrorContext::change("Start ED server", "service init"); - ros::AdvertiseServiceOptions opt_simple_query = - ros::AdvertiseServiceOptions::create( - "simple_query", srvSimpleQuery, ros::VoidPtr(), &cb_queue); - ros::ServiceServer srv_simple_query = nh_private.advertiseService(opt_simple_query); + rclcpp::CallbackGroup::SharedPtr const cb_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - ros::AdvertiseServiceOptions opt_reset = - ros::AdvertiseServiceOptions::create( - "reset", srvReset, ros::VoidPtr(), &cb_queue); - ros::ServiceServer srv_reset = nh_private.advertiseService(opt_reset); + auto srv_simple_query = node->create_service( + "~/simple_query", &srvSimpleQuery, rclcpp::ServicesQoS(), cb_group); + auto srv_reset = + node->create_service("~/reset", &srvReset, rclcpp::ServicesQoS(), cb_group); + auto srv_query = + node->create_service("~/query", &srvQuery, rclcpp::ServicesQoS(), cb_group); + auto srv_update = + node->create_service("~/update", &srvUpdate, rclcpp::ServicesQoS(), cb_group); + auto srv_configure = node->create_service( + "~/configure", &srvConfigure, rclcpp::ServicesQoS(), cb_group); - ros::NodeHandle nh_private2("~"); - nh_private2.setCallbackQueue(&cb_queue); - - ros::ServiceServer srv_query = nh_private2.advertiseService("query", srvQuery); - ros::ServiceServer srv_update = nh_private2.advertiseService("update", srvUpdate); - ros::ServiceServer srv_configure = nh_private2.advertiseService("configure", srvConfigure); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_callback_group(cb_group, node->get_node_base_interface()); // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - errc.change("Start ED server", "init"); + ed::ErrorContext::change("Start ED server", "init"); // Init ED ed_wm->initialize(); @@ -648,13 +645,14 @@ int main(int argc, char** argv) // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - errc.change("ED server", "main loop"); + ed::ErrorContext::change("ED server", "main loop"); - ros::Rate r(1000); - while(ros::ok()) { + rclcpp::WallRate r(1000); + while (rclcpp::ok()) + { if (trigger_cb.triggers()) - cb_queue.callAvailable(); + executor.spin_some(); // Check if configuration has changed. If so, call reconfigure if (trigger_config.triggers() && config.sync()) @@ -672,5 +670,7 @@ int main(int argc, char** argv) r.sleep(); } + rclcpp::shutdown(); + return 0; } diff --git a/src/entity.cpp b/src/entity.cpp index 154142f2..7089d5b4 100644 --- a/src/entity.cpp +++ b/src/entity.cpp @@ -2,10 +2,23 @@ #include "ed/measurement.h" -#include +#include +#include +#include #include +#include + +#include +#include +#include +#include +#include +#include #include "ed/convex_hull_calc.h" +#include "ed/measurement_convex_hull.h" +#include "ed/types.h" +#include "ed/uuid.h" // ---------------------------------------------------------------------------------------------------- @@ -14,20 +27,8 @@ namespace ed // ---------------------------------------------------------------------------------------------------- -Entity::Entity(const UUID& id, const TYPE& type, const unsigned int& measurement_buffer_size) : - id_(id), - revision_(0), - type_(type), - existence_prob_(1.0), - last_update_timestamp_(0), - measurements_(measurement_buffer_size), - measurements_seq_(0), - visual_revision_(0), - collision_revision_(0), - volumes_revision_(0), -// creation_time_(creation_time), - has_pose_(false), - pose_(geo::Pose3D::identity()) +Entity::Entity(UUID id, TYPE type, const unsigned int& measurement_buffer_size) : + id_(std::move(id)), type_(std::move(type)), measurements_(measurement_buffer_size), pose_(geo::Pose3D::identity()) { } @@ -35,7 +36,7 @@ Entity::Entity(const UUID& id, const TYPE& type, const unsigned int& measurement Entity::~Entity() { -// std::cout << "Removing entity with ID: " << id_ << std::endl; + // std::cout << "Removing entity with ID: " << id_ << std::endl; } // ---------------------------------------------------------------------------------------------------- @@ -48,7 +49,7 @@ void Entity::updateConvexHull() return; } - std::map::const_iterator it = convex_hull_map_.begin(); + auto it = convex_hull_map_.begin(); const MeasurementConvexHull& m = it->second; if (convex_hull_map_.size() == 1) @@ -66,16 +67,16 @@ void Entity::updateConvexHull() ++it; std::vector points; - for(; it != convex_hull_map_.end(); ++it) + for (; it != convex_hull_map_.end(); ++it) { const MeasurementConvexHull& m = it->second; z_min = std::min(z_min, m.convex_hull.z_min + m.pose.t.z); z_max = std::max(z_max, m.convex_hull.z_max + m.pose.t.z); - geo::Vec2f offset(m.pose.t.x, m.pose.t.y); + geo::Vec2f const offset(m.pose.t.x, m.pose.t.y); - for(unsigned int i = 0; i < m.convex_hull.points.size(); ++i) - points.push_back(m.convex_hull.points[i] + offset); + for (const auto& point : m.convex_hull.points) + points.push_back(point + offset); } ed::convex_hull::create(points, z_min, z_max, convex_hull_new_, pose_); @@ -96,12 +97,12 @@ void Entity::updateConvexHullFromVisual() float z_max = -1e9; std::vector points(vertices.size()); - for(unsigned int i = 0; i < vertices.size(); ++i) + for (unsigned int i = 0; i < vertices.size(); ++i) { -// geo::Vector3 p_MAP = pose_ * vertices[i]; + // geo::Vector3 p_MAP = pose_ * vertices[i]; // old implementation, this is correct, but gives the wrong result with the rest of the code // Because it is too much work for now to change that. So therefore ignoring rotation. - geo::Vector3 p_MAP = pose_.t + vertices[i]; + geo::Vector3 const p_MAP = pose_.t + vertices[i]; // new implementation, not correct either. Because this creates the wrong output in case of other rotation, // than arround z-axis. But solves the main issue, rotation of convex hull is in the wrong frame. // ToDo: Make sure everything in stamped correctly. Then conversion are much easier. @@ -138,10 +139,9 @@ void Entity::setCollision(const geo::ShapeConstPtr& collision) } } - // ---------------------------------------------------------------------------------------------------- -void Entity::addMeasurement(MeasurementConstPtr measurement) +void Entity::addMeasurement(const MeasurementConstPtr& measurement) { // Push back the measurement measurements_.push_front(measurement); @@ -150,8 +150,9 @@ void Entity::addMeasurement(MeasurementConstPtr measurement) // Update beste measurement if (best_measurement_) { - if (measurement->imageMask().getSize() > best_measurement_->imageMask().getSize() - || (measurement->mask() && best_measurement_->mask() && measurement->mask()->size() > best_measurement_->mask()->size())) + if (measurement->imageMask().getSize() > best_measurement_->imageMask().getSize() || + (measurement->mask() && best_measurement_->mask() && + measurement->mask()->size() > best_measurement_->mask()->size())) best_measurement_ = measurement; } else @@ -164,20 +165,18 @@ void Entity::addMeasurement(MeasurementConstPtr measurement) void Entity::measurements(std::vector& measurements, double min_timestamp) const { - for(boost::circular_buffer::const_iterator it = measurements_.begin(); it != measurements_.end(); ++it) + for (const auto& m : measurements_) { - const MeasurementConstPtr& m = *it; if (m->timestamp() > min_timestamp) measurements.push_back(m); } } - // ---------------------------------------------------------------------------------------------------- void Entity::measurements(std::vector& measurements, unsigned int num) const { - for(unsigned int i = 0; i < num && i < measurements_.size(); ++i) + for (unsigned int i = 0; i < num && i < measurements_.size(); ++i) { measurements.push_back(measurements_[i]); } @@ -188,25 +187,27 @@ void Entity::measurements(std::vector& measurements, unsign MeasurementConstPtr Entity::lastMeasurement() const { if (measurements_.empty()) - return MeasurementConstPtr(); + return {}; return measurements_.front(); } // ---------------------------------------------------------------------------------------------------- -UUID Entity::generateID() { +UUID Entity::generateID() +{ static const char alphanum[] = "0123456789" "abcdef"; std::string s; - for (int i = 0; i < 32; ++i) { - int n = rand() / (RAND_MAX / (sizeof(alphanum) - 1) + 1); + for (int i = 0; i < 32; ++i) + { + int const n = rand() / (RAND_MAX / (sizeof(alphanum) - 1) + 1); s += alphanum[n]; } - return UUID(s); + return {s}; } -} +} // namespace ed diff --git a/src/error_context.cpp b/src/error_context.cpp index 5689f173..c57c79a3 100644 --- a/src/error_context.cpp +++ b/src/error_context.cpp @@ -1,7 +1,8 @@ #include "ed/error_context.h" +#include #include -#include +#include namespace ed { @@ -13,24 +14,21 @@ namespace void dataDestructor(void* data) { - ErrorContextData* edata = static_cast(data); + auto const* edata = static_cast(data); delete edata; } struct KeyHolder { - KeyHolder() { - pthread_key_create(&key, &dataDestructor); - } - - pthread_key_t key; + KeyHolder() { pthread_key_create(&key, &dataDestructor); } + pthread_key_t key{}; }; - static KeyHolder key; +KeyHolder key; -} +} // namespace // ---------------------------------------------------------------------------------------------------- @@ -43,8 +41,7 @@ ErrorContext::ErrorContext(const char* msg, const char* value) pthread_setspecific(key.key, _data); } - _data->stack.push_back(std::pair(msg, value)); - + _data->stack.emplace_back(msg, value); } ErrorContext::~ErrorContext() @@ -67,4 +64,4 @@ ErrorContextData* ErrorContext::data() return static_cast(pthread_getspecific(key.key)); } -} +} // namespace ed diff --git a/src/io/filesystem/read.cpp b/src/io/filesystem/read.cpp index cadf9ab6..847da08c 100644 --- a/src/io/filesystem/read.cpp +++ b/src/io/filesystem/read.cpp @@ -3,21 +3,30 @@ #include "ed/measurement.h" #include "ed/serialization/serialization.h" +#include +#include +#include +#include #include #include +#include +#include +#include #include #include #include "ed/io/json_reader.h" +#include "ed/convex_hull_calc.h" #include "ed/entity.h" -#include "ed/update_request.h" #include "ed/logging.h" -#include "ed/convex_hull_calc.h" +#include "ed/types.h" +#include "ed/update_request.h" +#include "ed/uuid.h" -#include +#include namespace ed { @@ -34,8 +43,8 @@ rgbd::ImagePtr readRGBDImage(const std::string& filename) if (!f_in.is_open()) { - std::cout << "Could not open '" << filename << "'." << std::endl; - return rgbd::ImagePtr(); + std::cout << "Could not open '" << filename << "'." << '\n'; + return {}; } tue::serialization::InputArchive a_in(f_in); @@ -51,7 +60,7 @@ bool readImageMask(const std::string& filename, ed::ImageMask& mask) if (!f_in.is_open()) { - std::cout << "Could not open '" << filename << "'." << std::endl; + std::cout << "Could not open '" << filename << "'." << '\n'; return false; } @@ -61,14 +70,14 @@ bool readImageMask(const std::string& filename, ed::ImageMask& mask) return true; } -} +} // namespace // ---------------------------------------------------------------------------------------------------- bool read(const std::string& filename, Measurement& msr) { // Read image - rgbd::ImagePtr image = readRGBDImage(filename + ".rgbd"); + rgbd::ImagePtr const image = readRGBDImage(filename + ".rgbd"); // Read mask ed::ImageMask mask; @@ -88,13 +97,13 @@ bool readEntity(const std::string& filename, UpdateRequest& req) if (!f_in.is_open()) { - std::cout << "Could not open '" << filename << "'." << std::endl; + std::cout << "Could not open '" << filename << "'." << '\n'; return false; } std::stringstream buffer; buffer << f_in.rdbuf(); - std::string str = buffer.str(); + std::string const str = buffer.str(); io::JSONReader r(str.c_str()); @@ -129,9 +138,9 @@ bool readEntity(const std::string& filename, UpdateRequest& req) if (r.readArray("points")) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { - chull.points.push_back(geo::Vec2f()); + chull.points.emplace_back(); geo::Vec2f& p = chull.points.back(); r.readValue("x", p.x); r.readValue("y", p.y); @@ -142,7 +151,7 @@ bool readEntity(const std::string& filename, UpdateRequest& req) ed::convex_hull::calculateEdgesAndNormals(chull); ed::convex_hull::calculateArea(chull); - ed::log::warning() << "ed::readEntity: convex hull timestamp is set to 0." << std::endl; + ed::log::warning() << "ed::readEntity: convex hull timestamp is set to 0." << '\n'; req.setConvexHullNew(id, chull, pose, 0); r.endGroup(); @@ -151,12 +160,15 @@ bool readEntity(const std::string& filename, UpdateRequest& req) // RGBD measurement if (r.readGroup("rgbd_measurement")) { - std::string rgbd_filename, mask_filename; + std::string rgbd_filename; + std::string mask_filename; if (r.readValue("image_file", rgbd_filename) && r.readValue("mask_file", mask_filename)) { - std::string base_path = tue::filesystem::Path(filename).parentPath().string(); + // tue::filesystem::Path::parentPath() returned "." for a bare filename; std::filesystem returns empty + std::filesystem::path const parent_path = std::filesystem::path(filename).parent_path(); + std::string const base_path = parent_path.empty() ? "." : parent_path.string(); - rgbd::ImagePtr image = readRGBDImage(base_path + "/" + rgbd_filename); + rgbd::ImagePtr const image = readRGBDImage(base_path + "/" + rgbd_filename); // Read mask ed::ImageMask mask; @@ -170,11 +182,11 @@ bool readEntity(const std::string& filename, UpdateRequest& req) } else { - log::error() << "Could not read sensor pose from rgbd measurement" << std::endl; + log::error() << "Could not read sensor pose from rgbd measurement" << '\n'; sensor_pose = geo::Pose3D::identity(); } - MeasurementPtr msr(new Measurement(image, mask, sensor_pose)); + MeasurementPtr const msr(new Measurement(image, mask, sensor_pose)); req.addMeasurement(id, msr); } @@ -185,4 +197,4 @@ bool readEntity(const std::string& filename, UpdateRequest& req) return true; } -} +} // namespace ed diff --git a/src/io/filesystem/write.cpp b/src/io/filesystem/write.cpp index ceefcb8b..186b4f29 100644 --- a/src/io/filesystem/write.cpp +++ b/src/io/filesystem/write.cpp @@ -1,13 +1,17 @@ #include "ed/io/filesystem/write.h" -#include "ed/measurement.h" #include "ed/entity.h" -#include "ed/serialization/serialization.h" #include "ed/io/json_writer.h" #include "ed/logging.h" +#include "ed/measurement.h" +#include "ed/serialization/serialization.h" +#include "ed/types.h" +#include +#include +#include +#include #include -#include #include @@ -22,7 +26,7 @@ bool write(const std::string& filename, const Measurement& msr) { // save image { - std::string filename_image = filename + ".rgbd"; + std::string const filename_image = filename + ".rgbd"; std::ofstream f_out; f_out.open(filename_image.c_str(), std::ifstream::binary); if (f_out.is_open()) @@ -32,13 +36,13 @@ bool write(const std::string& filename, const Measurement& msr) } else { - std::cout << "Could not save to " << filename_image << std::endl; + std::cout << "Could not save to " << filename_image << '\n'; } } // save mask { - std::string filename_mask = filename + ".mask"; + std::string const filename_mask = filename + ".mask"; std::ofstream f_out; f_out.open(filename_mask.c_str(), std::ifstream::binary); if (f_out.is_open()) @@ -48,7 +52,7 @@ bool write(const std::string& filename, const Measurement& msr) } else { - std::cout << "Could not save to " << filename_mask << std::endl; + std::cout << "Could not save to " << filename_mask << '\n'; } } @@ -59,13 +63,13 @@ bool write(const std::string& filename, const Measurement& msr) bool write(const std::string& filename, const Entity& e) { - std::string filename_ext = filename + ".json"; + std::string const filename_ext = filename + ".json"; std::ofstream f_out; f_out.open(filename_ext.c_str()); if (!f_out.is_open()) { - ed::log::error() << "Could not save to '" << filename_ext << "'" << std::endl; + ed::log::error() << "Could not save to '" << filename_ext << "'" << '\n'; return false; } @@ -92,13 +96,13 @@ bool write(const std::string& filename, const Entity& e) } // RGBD Measurement - ed::MeasurementConstPtr msr = e.lastMeasurement(); + ed::MeasurementConstPtr const msr = e.lastMeasurement(); if (msr) { w.writeGroup("rgbd_measurement"); // Get filename without path - std::string base_filename = tue::filesystem::Path(filename).filename(); + std::string const base_filename = std::filesystem::path(filename).filename().string(); w.writeValue("image_file", base_filename + ".rgbd"); w.writeValue("mask_file", base_filename + ".mask"); @@ -117,4 +121,4 @@ bool write(const std::string& filename, const Entity& e) return true; } -} +} // namespace ed diff --git a/src/io/json_reader.cpp b/src/io/json_reader.cpp index 936e1e30..e223bbfa 100644 --- a/src/io/json_reader.cpp +++ b/src/io/json_reader.cpp @@ -1,36 +1,62 @@ #include "ed/io/json_reader.h" -#include "rapidjson/reader.h" +#include "ed/io/data.h" #include "ed/io/data_writer.h" +#include "rapidjson/rapidjson.h" +#include "rapidjson/reader.h" +#include +#include +#include - -namespace ed +namespace ed::io { -namespace io +// ---------------------------------------------------------------------------------------------------- + +struct MyHandler { -// ---------------------------------------------------------------------------------------------------- + MyHandler(ed::io::DataWriter& w_) : w(w_) {} -struct MyHandler { + static bool Null() { return true; } - MyHandler(ed::io::DataWriter& w_) : w(w_) + bool Bool(bool b) { + int const i = b; + w.setValue(key, i); + ; + return true; } - bool Null() { return true; } - - bool Bool(bool b) { int i = b; w.setValue(key, i); ; return true; } - - bool Int(int i) { w.setValue(key, i); return true; } + bool Int(int i) + { + w.setValue(key, i); + return true; + } - bool Uint(unsigned u) { w.setValue(key, (int)u); return true; } + bool Uint(unsigned u) + { + w.setValue(key, static_cast(u)); + return true; + } - bool Int64(int64_t i) { w.setValue(key, (int)i); return true; } + bool Int64(int64_t i) + { + w.setValue(key, static_cast(i)); + return true; + } - bool Uint64(uint64_t u) { w.setValue(key, (int)u); return true; } + bool Uint64(uint64_t u) + { + w.setValue(key, static_cast(u)); + return true; + } - bool Double(double d) { w.setValue(key, d); return true; } + bool Double(double d) + { + w.setValue(key, d); + return true; + } bool RawNumber(const char* str, rapidjson::SizeType /*len*/, bool /*copy*/) { @@ -64,7 +90,11 @@ struct MyHandler { return true; } - bool Key(const char* str, rapidjson::SizeType /*length*/, bool /*copy*/) { key = str; return true; } + bool Key(const char* str, rapidjson::SizeType /*length*/, bool /*copy*/) + { + key = str; + return true; + } bool EndObject(rapidjson::SizeType /*memberCount*/) { @@ -97,7 +127,6 @@ struct MyHandler { ed::io::DataWriter& w; std::string key; std::vector stack; - }; // ---------------------------------------------------------------------------------------------------- @@ -119,16 +148,14 @@ JSONReader::JSONReader(const char* s) : n_current_(Node(0, MAP)) // ---------------------------------------------------------------------------------------------------- -JSONReader::~JSONReader() -{ -} +JSONReader::~JSONReader() = default; // ---------------------------------------------------------------------------------------------------- bool JSONReader::readGroup(const std::string& key) { std::map& map = data_.maps[n_current_.idx]; - std::map::const_iterator it = map.find(key); + auto const it = map.find(key); if (it == map.end()) return false; @@ -150,7 +177,7 @@ bool JSONReader::endGroup() bool JSONReader::readArray(const std::string& key) { std::map& map = data_.maps[n_current_.idx]; - std::map::const_iterator it = map.find(key); + auto const it = map.find(key); if (it == map.end()) return false; @@ -167,7 +194,7 @@ bool JSONReader::endArray() if (array_index_stack_.empty()) return false; - unsigned int& i_next_array_item_ = array_index_stack_.back(); + unsigned int const& i_next_array_item_ = array_index_stack_.back(); array_index_stack_.pop_back(); if (n_current_.type != ARRAY && i_next_array_item_ > 0) @@ -221,7 +248,6 @@ bool JSONReader::readValue(const std::string& key, float& f) bool JSONReader::readValue(const std::string& key, double& d) { return value(key, d); - } // ---------------------------------------------------------------------------------------------------- @@ -238,6 +264,4 @@ bool JSONReader::readValue(const std::string& key, std::string& s) return value(key, s); } -} - -} +} // namespace ed::io diff --git a/src/io/transport/probe.cpp b/src/io/transport/probe.cpp index 4e8025d7..1e1b8b0e 100644 --- a/src/io/transport/probe.cpp +++ b/src/io/transport/probe.cpp @@ -1,39 +1,47 @@ #include "ed/io/transport/probe.h" - -#include -#include - +#include "ed/types.h" +#include "tue_serialization_interfaces/srv/binary_service.hpp" + +#include +#include +#include +#include +#include #include +#include +#include +#include + namespace ed { // ---------------------------------------------------------------------------------------------------- -Probe::Probe() -{ -} +Probe::Probe() = default; // ---------------------------------------------------------------------------------------------------- -Probe::~Probe() -{ -} +Probe::~Probe() = default; // ---------------------------------------------------------------------------------------------------- void Probe::initialize() { - ros::NodeHandle nh; + cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - ros::AdvertiseServiceOptions opt_srv = - ros::AdvertiseServiceOptions::create( - "ed/probe/" + name(), boost::bind(&Probe::srvCallback, this, _1, _2), - ros::VoidPtr(), &cb_queue_); + // std::bind is required here: rclcpp deduces the service callback signature from the concrete + // argument types, which a generic `auto&&` lambda does not provide. + srv_ = node_->create_service( + "ed/probe/" + name(), + std::bind( + &Probe::srvCallback, this, std::placeholders::_1, std::placeholders::_2), // NOLINT(modernize-avoid-bind) + rclcpp::ServicesQoS(), + cb_group_); - srv_ = nh.advertiseService(opt_srv); + executor_.add_callback_group(cb_group_, node_->get_node_base_interface()); - std::cout << "Probe '" << name() << "' initialized." << std::endl; + std::cout << "Probe '" << name() << "' initialized." << '\n'; } // ---------------------------------------------------------------------------------------------------- @@ -43,16 +51,17 @@ void Probe::process(const WorldModel& world, UpdateRequest& req) world_ = &world; update_req_ = &req; - cb_queue_.callAvailable(); + executor_.spin_some(); } // ---------------------------------------------------------------------------------------------------- -bool Probe::srvCallback(const tue_serialization::BinaryService::Request& ros_req, - tue_serialization::BinaryService::Response& ros_res) +// NOLINTNEXTLINE(performance-unnecessary-value-param) - rclcpp service callback requires shared_ptr by value +void Probe::srvCallback(const std::shared_ptr ros_req, + const std::shared_ptr& ros_res) { std::stringstream ss_req; - tue::serialization::convert(ros_req.bin.data, ss_req); + tue::serialization::convert(ros_req->bin.data, ss_req); tue::serialization::InputArchive req(ss_req); std::stringstream ss_res; @@ -60,9 +69,7 @@ bool Probe::srvCallback(const tue_serialization::BinaryService::Request& ros_req this->process(*world_, *update_req_, req, res); - tue::serialization::convert(ss_res, ros_res.bin.data); - - return true; + tue::serialization::convert(ss_res, ros_res->bin.data); } -} +} // namespace ed diff --git a/src/io/transport/probe_client.cpp b/src/io/transport/probe_client.cpp index e5f58afd..45ffd360 100644 --- a/src/io/transport/probe_client.cpp +++ b/src/io/transport/probe_client.cpp @@ -1,10 +1,18 @@ #include "ed/io/transport/probe_client.h" // ROS services -#include -#include "tue_serialization/BinaryService.h" - -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include @@ -13,34 +21,26 @@ namespace ed // ---------------------------------------------------------------------------------------------------- -ProbeClient::ProbeClient() : nh_(nullptr) -{ -} +ProbeClient::ProbeClient() = default; // ---------------------------------------------------------------------------------------------------- -ProbeClient::~ProbeClient() -{ - delete nh_; -} +ProbeClient::~ProbeClient() = default; // ---------------------------------------------------------------------------------------------------- void ProbeClient::launchProbe(const std::string& probe_name, const std::string& lib) { - if (!ros::isInitialized()) - { - ros::M_string remapping_args; - ros::init(remapping_args, "ed_probe_client_" + probe_name); - } + if (!rclcpp::ok()) + rclcpp::init(0, nullptr); - nh_ = new ros::NodeHandle(); - ros::ServiceClient client = nh_->serviceClient("ed/configure"); - client.waitForExistence(); + node_ = rclcpp::Node::make_shared("ed_probe_client_" + probe_name); + auto client = node_->create_client("ed/configure"); + client->wait_for_service(); - ed_msgs::Configure srv; + auto request = std::make_shared(); - double freq = 1000; // default + double const freq = 1000; // default tue::Configuration config; config.writeArray("plugins"); @@ -55,15 +55,16 @@ void ProbeClient::launchProbe(const std::string& probe_name, const std::string& } config.endArray(); - srv.request.request = config.toYAMLString(); + request->request = config.toYAMLString(); - std::cout << "Sending request to launch probe using configuration: " << srv.request.request << std::endl; + std::cout << "Sending request to launch probe using configuration: " << request->request << '\n'; std::string error; - if (client.call(srv)) + auto future = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, future) == rclcpp::FutureReturnCode::SUCCESS) { - error = srv.response.error_msg; + error = future.get()->error_msg; } else { @@ -72,47 +73,43 @@ void ProbeClient::launchProbe(const std::string& probe_name, const std::string& if (!error.empty()) { - std::cout << "[ed::ProbeClient] ERROR: " + error << std::endl; + std::cout << "[ed::ProbeClient] ERROR: " + error << '\n'; } else { // Initialize connection with the probe probe_name_ = probe_name; - srv_probe_ = nh_->serviceClient("ed/probe/" + probe_name_); - srv_probe_.waitForExistence(); + srv_probe_ = node_->create_client("ed/probe/" + probe_name_); + srv_probe_->wait_for_service(); } } // ---------------------------------------------------------------------------------------------------- -void ProbeClient::configure(tue::Configuration /*config*/) -{ - -} +void ProbeClient::configure(const tue::Configuration& /*config*/) {} // ---------------------------------------------------------------------------------------------------- bool ProbeClient::process(tue::serialization::Archive& req, tue::serialization::Archive& res) { - if (!srv_probe_.exists()) + if (!srv_probe_ || !srv_probe_->service_is_ready()) { - std::cout << "Service does not exist" << std::endl; + std::cout << "Service does not exist" << '\n'; return false; } - tue_serialization::BinaryService srv; - tue::serialization::convert(req, srv.request.bin.data); + auto request = std::make_shared(); + tue::serialization::convert(req, request->bin.data); - if (srv_probe_.call(srv)) + auto future = srv_probe_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, future) == rclcpp::FutureReturnCode::SUCCESS) { - tue::serialization::convert(srv.response.bin.data, res); + tue::serialization::convert(future.get()->bin.data, res); return true; } - else - { - std::cout << "Service call failed" << std::endl; - return false; - } -} + std::cout << "Service call failed" << '\n'; + return false; } + +} // namespace ed diff --git a/src/io/transport/probe_ros.cpp b/src/io/transport/probe_ros.cpp index adc339c3..954658c9 100644 --- a/src/io/transport/probe_ros.cpp +++ b/src/io/transport/probe_ros.cpp @@ -5,14 +5,10 @@ namespace ed // ---------------------------------------------------------------------------------------------------- -ProbeROS::ProbeROS() -{ -} +ProbeROS::ProbeROS() = default; // ---------------------------------------------------------------------------------------------------- -ProbeROS::~ProbeROS() -{ -} +ProbeROS::~ProbeROS() = default; -} +} // namespace ed diff --git a/src/logging.cpp b/src/logging.cpp index f1b533ca..376b5352 100644 --- a/src/logging.cpp +++ b/src/logging.cpp @@ -1,28 +1,27 @@ #include "ed/logging.h" +#include #include #include +#include namespace { - std::string prefix() - { - std::string ps = "[ED] "; - - // Add thread name - char name[1000]; - size_t name_size = 1000; - if (pthread_getname_np(pthread_self(), name, name_size) == 0) - ps += "(" + std::string(name) + ") "; - - return ps; - } -} - -namespace ed +std::string prefix() { + std::string ps = "[ED] "; + + // Add thread name + char name[1000]; + size_t const name_size = 1000; + if (pthread_getname_np(pthread_self(), name, name_size) == 0) + ps += "(" + std::string(name) + ") "; + + return ps; +} +} // namespace -namespace log +namespace ed::log { // ---------------------------------------------------------------------------------------------------- @@ -37,7 +36,7 @@ std::ostream& info() void info(const char* str) { - std::cout << "\e[1;37m" << prefix() << str << " \e[0m" << std::endl; + std::cout << "\e[1;37m" << prefix() << str << " \e[0m" << '\n'; } // ---------------------------------------------------------------------------------------------------- @@ -59,7 +58,7 @@ std::ostream& warning() void warning(const char* str) { - std::cout << "\e[1;33m" << prefix() << "Warning: \e[0m" << str << std::endl; + std::cout << "\e[1;33m" << prefix() << "Warning: \e[0m" << str << '\n'; } // ---------------------------------------------------------------------------------------------------- @@ -81,7 +80,7 @@ std::ostream& error() void error(const char* str) { - std::cout << "\e[1;31m" << prefix() << "Error: \e[0m" << str << std::endl; + std::cout << "\e[1;31m" << prefix() << "Error: \e[0m" << str << '\n'; } // ---------------------------------------------------------------------------------------------------- @@ -93,6 +92,4 @@ void error(const std::string& str) // ---------------------------------------------------------------------------------------------------- -} - -} +} // namespace ed::log diff --git a/src/measurement.cpp b/src/measurement.cpp index ce5fcc2c..017c98f7 100644 --- a/src/measurement.cpp +++ b/src/measurement.cpp @@ -1,21 +1,24 @@ #include "ed/measurement.h" +#include "ed/mask.h" +#include "ed/rgbd_data.h" -#include +#include +#include + +#include +#include namespace ed { // ---------------------------------------------------------------------------------------------------- -Measurement::Measurement() : timestamp_(0) -{ -} +Measurement::Measurement() : timestamp_(0) {} // ---------------------------------------------------------------------------------------------------- -Measurement::Measurement(rgbd::ImageConstPtr image, const ImageMask& image_mask, const geo::Pose3D& sensor_pose) : - image_mask_(image_mask), - timestamp_(image->getTimestamp()) +Measurement::Measurement(const rgbd::ImageConstPtr& image, ImageMask image_mask, const geo::Pose3D& sensor_pose) : + image_mask_(std::move(image_mask)), timestamp_(image->getTimestamp()) { rgbd_data_.image = image; rgbd_data_.sensor_pose = sensor_pose; @@ -23,21 +26,17 @@ Measurement::Measurement(rgbd::ImageConstPtr image, const ImageMask& image_mask, // ---------------------------------------------------------------------------------------------------- -Measurement::Measurement(const RGBDData& rgbd_data, const PointCloudMaskPtr& mask, unsigned int seq) : - rgbd_data_(rgbd_data), - mask_(mask), - timestamp_(rgbd_data.image->getTimestamp()), - seq_(seq) +Measurement::Measurement(const RGBDData& rgbd_data, PointCloudMaskPtr mask, unsigned int seq) : + rgbd_data_(rgbd_data), mask_(std::move(mask)), timestamp_(rgbd_data.image->getTimestamp()), seq_(seq) { // Calculate image mask image_mask_.setSize(rgbd_data.image->getDepthImage().cols, rgbd_data.image->getDepthImage().rows); - for(PointCloudMask::const_iterator it = mask_->begin(); it != mask_->end(); ++it) + for (int const it : *mask_) { - const std::vector& pixel_idxs = rgbd_data_.point_cloud_to_pixels_mapping[*it]; - for(std::vector::const_iterator it2 = pixel_idxs.begin(); it2 != pixel_idxs.end(); ++it2) - image_mask_.addPoint(*it2); + const std::vector& pixel_idxs = rgbd_data_.point_cloud_to_pixels_mapping[it]; + for (int const pixel_idx : pixel_idxs) + image_mask_.addPoint(pixel_idx); } - } -} +} // namespace ed diff --git a/src/models/load_model.cpp b/src/models/load_model.cpp index 8ce364ab..81d0f0af 100644 --- a/src/models/load_model.cpp +++ b/src/models/load_model.cpp @@ -1,22 +1,23 @@ +#include #include -// ROS -#include +// ED +#include "ed/logging.h" // TU/e Robotics -#include -#include +#include +#include #include #include #include +#include // ED -#include "ed/update_request.h" #include "ed/models/model_loader.h" +#include "ed/update_request.h" -namespace ed { - -namespace models { +namespace ed::models +{ bool loadModel(const enum LoadType load_type, const std::string& source, ed::UpdateRequest& req) { @@ -24,16 +25,16 @@ bool loadModel(const enum LoadType load_type, const std::string& source, ed::Upd std::stringstream error; if (load_type == LoadType::FILE) { - tue::filesystem::Path path(source); - if (!path.exists()) + std::filesystem::path const path(source); + if (!std::filesystem::exists(path)) { - ROS_ERROR_STREAM("Couldn't open: '" << path << "', because it doesn't exist"); + ed::log::error() << "Couldn't open: '" << source << "', because it doesn't exist" << '\n'; return false; } tue::config::ReaderWriter config; - std::string extension = tue::filesystem::Path(source).extension(); - if ( extension == ".sdf" || extension == ".world") + std::string const extension = std::filesystem::path(source).extension().string(); + if (extension == ".sdf" || extension == ".world") tue::config::loadFromSDFFile(source, config); else if (extension == ".xml") tue::config::loadFromXMLFile(source, config); @@ -41,14 +42,13 @@ bool loadModel(const enum LoadType load_type, const std::string& source, ed::Upd tue::config::loadFromYAMLFile(source, config); else { - ROS_ERROR_STREAM("[model_viewer] extension: '" << extension << "' is not supported."); + ed::log::error() << "[model_viewer] extension: '" << extension << "' is not supported." << '\n'; return false; } if (!model_loader.create(config.data(), req, error)) { - ROS_ERROR_STREAM("File '" << source << "' could not be loaded:" << - "\nError:\n" << error.str()); + ed::log::error() << "File '" << source << "' could not be loaded:" << "\nError:\n" << error.str() << '\n'; return false; } } @@ -56,21 +56,17 @@ bool loadModel(const enum LoadType load_type, const std::string& source, ed::Upd { if (!model_loader.create("_root", source, req, error, true)) { - ROS_ERROR_STREAM("Model '" << source << "' could not be loaded:" << - "\nError:\n" << error.str()); + ed::log::error() << "Model '" << source << "' could not be loaded:" << "\nError:\n" << error.str() << '\n'; return false; } } else { - ROS_ERROR_STREAM("Unknown load type"); + ed::log::error() << "Unknown load type" << '\n'; return false; } return true; - } -} // End of namespace 'models' - -} // End of namespace 'ed' +} // namespace ed::models diff --git a/src/models/model_loader.cpp b/src/models/model_loader.cpp index 3f6c5e39..b0e6d5da 100644 --- a/src/models/model_loader.cpp +++ b/src/models/model_loader.cpp @@ -2,31 +2,39 @@ #include "ed/types.h" -#include "ed/update_request.h" #include "ed/entity.h" -#include "ed/relations/transform_cache.h" +#include "ed/update_request.h" -#include +#include +#include +#include #include "shape_loader_private.h" -#include -#include +#include +#include +#include #include +#include +#include +#include +#include #include +#include #include #include +#include -namespace ed -{ - -namespace models +namespace ed::models { -bool readSDFGeometry(tue::config::Reader r, geo::CompositeShapePtr& composite, std::stringstream& error, geo::Pose3D pose_offset=geo::Pose3D::identity()) +static bool readSDFGeometry(tue::config::Reader r, + geo::CompositeShapePtr& composite, + std::stringstream& error, + const geo::Pose3D& pose_offset = geo::Pose3D::identity()) { geo::Pose3D pose = geo::Pose3D::identity(); readPose(r, pose); @@ -35,11 +43,11 @@ bool readSDFGeometry(tue::config::Reader r, geo::CompositeShapePtr& composite, s return false; std::map dummy_shape_cache; - geo::ShapePtr sub_shape = loadShape("", r, dummy_shape_cache, error); + geo::ShapePtr const sub_shape = loadShape("", r, dummy_shape_cache, error); if (sub_shape) { if (!composite) // if pointer is empty, create new instance. - composite.reset(new geo::CompositeShape); + composite = std::make_shared(); composite->addShape(*sub_shape, pose); } r.endGroup(); @@ -50,34 +58,32 @@ bool readSDFGeometry(tue::config::Reader r, geo::CompositeShapePtr& composite, s ModelLoader::ModelLoader() { - const char * edmpath = ::getenv("ED_MODEL_PATH"); + const char* edmpath = ::getenv("ED_MODEL_PATH"); if (edmpath) { - std::vector paths_vector = ed::models::split(edmpath, ':'); - for (std::vector::const_iterator it = paths_vector.begin(); it != paths_vector.end(); ++it) - ed_model_paths_.push_back(*it); + std::vector const paths_vector = ed::models::split(edmpath, ':'); + for (const auto& it : paths_vector) + ed_model_paths_.push_back(it); } - const char * mpath = ::getenv("GAZEBO_MODEL_PATH"); + const char* mpath = ::getenv("GAZEBO_MODEL_PATH"); if (mpath) { - std::vector paths_vector = ed::models::split(mpath, ':'); - for (std::vector::const_iterator it = paths_vector.begin(); it != paths_vector.end(); ++it) - model_paths_.push_back(*it); + std::vector const paths_vector = ed::models::split(mpath, ':'); + for (const auto& it : paths_vector) + model_paths_.push_back(it); } - const char * fpath = ::getenv("GAZEBO_RESOURCE_PATH"); + const char* fpath = ::getenv("GAZEBO_RESOURCE_PATH"); if (fpath) { - std::vector paths_vector = ed::models::split(fpath, ':'); - for (std::vector::const_iterator it = paths_vector.begin(); it != paths_vector.end(); ++it) - file_paths_.push_back(*it); + std::vector const paths_vector = ed::models::split(fpath, ':'); + for (const auto& it : paths_vector) + file_paths_.push_back(it); } } // ---------------------------------------------------------------------------------------------------- -ModelLoader::~ModelLoader() -{ -} +ModelLoader::~ModelLoader() = default; // ---------------------------------------------------------------------------------------------------- @@ -89,10 +95,10 @@ ModelLoader::~ModelLoader() std::string ModelLoader::getModelPath(const std::string& type) const { - for(std::vector::const_iterator it = ed_model_paths_.cbegin(); it != ed_model_paths_.cend(); ++it) + for (const auto& ed_model_path : ed_model_paths_) { - tue::filesystem::Path model_path(*it + "/" + type); - if (model_path.exists()) + std::filesystem::path const model_path(ed_model_path + "/" + type); + if (std::filesystem::exists(model_path)) return model_path.string(); } @@ -104,22 +110,22 @@ std::string ModelLoader::getModelPath(const std::string& type) const std::string ModelLoader::getSDFPath(const std::string& uri) const { ModelOrFile uri_type; - std::string parsed_uri = parseURI(uri, uri_type); + std::string const parsed_uri = parseURI(uri, uri_type); if (parsed_uri.empty()) return ""; if (uri_type == MODEL) { - for(std::vector::const_iterator it = model_paths_.cbegin(); it != model_paths_.cend(); ++it) + for (const auto& it : model_paths_) { - tue::filesystem::Path model_dir(*it + "/" + parsed_uri); - if (model_dir.exists()) + std::filesystem::path const model_dir(it + "/" + parsed_uri); + if (std::filesystem::exists(model_dir)) { - tue::filesystem::Path config_path(model_dir.string() + "/model.config"); - if (config_path.exists()) + std::filesystem::path const config_path(model_dir.string() + "/model.config"); + if (std::filesystem::exists(config_path)) { - tue::filesystem::Path model_path = sdf::getModelFilePath(model_dir.string()); - if (model_path.exists()) + std::filesystem::path const model_path = sdf::getModelFilePath(model_dir.string()); + if (std::filesystem::exists(model_path)) return model_path.string(); } } @@ -127,10 +133,10 @@ std::string ModelLoader::getSDFPath(const std::string& uri) const } if (uri_type == FILE) { - for(std::vector::const_iterator it = file_paths_.cbegin(); it != file_paths_.cend(); ++it) + for (const auto& it : file_paths_) { - tue::filesystem::Path file_path(*it + "/" + parsed_uri); - if (file_path.exists()) + std::filesystem::path const file_path(it + "/" + parsed_uri); + if (std::filesystem::exists(file_path)) return file_path.string(); } } @@ -140,21 +146,23 @@ std::string ModelLoader::getSDFPath(const std::string& uri) const // ---------------------------------------------------------------------------------------------------- -ModelLoader::ModelData ModelLoader::readModelCache(std::string type) const +ModelLoader::ModelData ModelLoader::readModelCache(const std::string& type) const { - std::map::const_iterator it = model_cache_.find(type); + auto const it = model_cache_.find(type); if (it != model_cache_.end()) return it->second; - tue::config::DataConstPointer data; - std::vector types; - return ModelData(data, types); + tue::config::DataConstPointer const data; + std::vector const types; + return ModelData(data, types); } // ---------------------------------------------------------------------------------------------------- -tue::config::DataConstPointer ModelLoader::loadModelData(std::string type, std::vector& types, - std::stringstream& error, const bool allow_sdf) +tue::config::DataConstPointer ModelLoader::loadModelData(const std::string& type, + std::vector& types, + std::stringstream& error, + const bool allow_sdf) { if (allow_sdf) { @@ -163,7 +171,7 @@ tue::config::DataConstPointer ModelLoader::loadModelData(std::string type, std:: if (!data_sdf.empty()) return data_sdf; } - ModelData cache_data = readModelCache(type); + ModelData const cache_data = readModelCache(type); if (!cache_data.first.empty()) { types = cache_data.second; @@ -172,31 +180,33 @@ tue::config::DataConstPointer ModelLoader::loadModelData(std::string type, std:: tue::config::DataPointer data; - std::string model_path = getModelPath(type); + std::string const model_path = getModelPath(type); if (model_path.empty()) { - error << "[ed::models::loadModelData] Model '" << type << "' could not be found." << std::endl; + error << "[ed::models::loadModelData] Model '" << type << "' could not be found." << '\n'; return data; } - tue::filesystem::Path model_cfg_path(model_path + "/model.yaml"); - if (!model_cfg_path.exists()) + std::filesystem::path const model_cfg_path(model_path + "/model.yaml"); + if (!std::filesystem::exists(model_cfg_path)) { - error << "[ed::models::loadModelData] ERROR loading configuration for model '" << type << "'; '" << model_cfg_path.string() << "' file does not exist." << std::endl; + error << "[ed::models::loadModelData] ERROR loading configuration for model '" << type << "'; '" + << model_cfg_path.string() << "' file does not exist." << '\n'; return data; } tue::Configuration model_cfg; if (!model_cfg.loadFromYAMLFile(model_cfg_path.string())) { - error << "[ed::models::loadModelData] ERROR loading configuration for model '" << type << "'; '" << model_cfg_path.string() << "' failed to parse yaml file." << std::endl; + error << "[ed::models::loadModelData] ERROR loading configuration for model '" << type << "'; '" + << model_cfg_path.string() << "' failed to parse yaml file." << '\n'; return data; } std::string super_type; if (model_cfg.value("type", super_type, tue::config::OPTIONAL)) { - tue::config::DataConstPointer super_data = loadModelData(super_type, types, error); + tue::config::DataConstPointer const super_data = loadModelData(super_type, types, error); tue::config::DataPointer combined_data; combined_data.add(super_data); combined_data.add(model_cfg.data()); @@ -222,39 +232,40 @@ tue::config::DataConstPointer ModelLoader::loadModelData(std::string type, std:: // ---------------------------------------------------------------------------------------------------- -tue::config::DataConstPointer ModelLoader::loadSDFData(std::string uri, std::stringstream& error) +tue::config::DataConstPointer ModelLoader::loadSDFData(const std::string& uri, std::stringstream& error) { tue::config::DataPointer data; ModelOrFile uri_type; - std::string parsed_uri = parseURI(uri, uri_type); + std::string const parsed_uri = parseURI(uri, uri_type); if (parsed_uri.empty()) { - error << "[ed::models::loadSDFData] Incorrect URI: '" << uri << "'." << std::endl; + error << "[ed::models::loadSDFData] Incorrect URI: '" << uri << "'." << '\n'; return data; } - ModelData cache_data = readModelCache(parsed_uri + "_sdf"); + ModelData const cache_data = readModelCache(parsed_uri + "_sdf"); if (!cache_data.first.empty()) { return cache_data.first; } - tue::filesystem::Path model_cfg_path = getSDFPath(uri); - if (!model_cfg_path.exists()) + std::filesystem::path const model_cfg_path = getSDFPath(uri); + if (!std::filesystem::exists(model_cfg_path)) { - error << "[ed::models::loadSDFData] Model '" << uri << "' could not be found." << std::endl; + error << "[ed::models::loadSDFData] Model '" << uri << "' could not be found." << '\n'; return data; } tue::Configuration model_cfg(data); if (!model_cfg.loadFromSDFFile(model_cfg_path.string())) { - error << "[ed::models::loadSDFData] ERROR loading configuration for model '" << uri << "'; '" << model_cfg_path << "' failed to parse SDF file." << std::endl; - error << model_cfg.error() << std::endl; + error << "[ed::models::loadSDFData] ERROR loading configuration for model '" << uri << "'; '" << model_cfg_path + << "' failed to parse SDF file." << '\n'; + error << model_cfg.error() << '\n'; return data; } // Store data in cache - model_cache_[parsed_uri+"_sdf"] = ModelData(data, std::vector()); + model_cache_[parsed_uri + "_sdf"] = ModelData(data, std::vector()); return data; } @@ -264,33 +275,32 @@ tue::config::DataConstPointer ModelLoader::loadSDFData(std::string uri, std::str bool ModelLoader::exists(const std::string& type) const { ModelData cache_data = readModelCache(type + "_sdf"); - if(!cache_data.first.empty()) + if (!cache_data.first.empty()) return true; cache_data = readModelCache(type); - if(!cache_data.first.empty()) + if (!cache_data.first.empty()) return true; - - std::string sdf_path = getSDFPath(type); + std::string const sdf_path = getSDFPath(type); if (!sdf_path.empty()) return true; - std::string model_path = getModelPath(type); + std::string const model_path = getModelPath(type); return !model_path.empty(); } // ---------------------------------------------------------------------------------------------------- -bool ModelLoader::create(const UUID& id, const std::string& type, UpdateRequest& req, std::stringstream& error, - const bool allow_sdf) +bool ModelLoader::create( + const UUID& id, const std::string& type, UpdateRequest& req, std::stringstream& error, const bool allow_sdf) { tue::config::DataConstPointer data; std::vector types; bool sdf = true; if (allow_sdf) data = loadSDFData("model://" + type, error); - if(data.empty()) + if (data.empty()) { sdf = false; data = loadModelData(type, types, error); @@ -311,8 +321,8 @@ bool ModelLoader::create(const UUID& id, const std::string& type, UpdateRequest& } types.push_back(type); - for(std::vector::const_iterator it = types.begin(); it != types.end(); ++it) - req.addType(id, *it); + for (const auto& type : types) + req.addType(id, type); return true; } @@ -326,8 +336,12 @@ bool ModelLoader::create(const tue::config::DataConstPointer& data, UpdateReques // ---------------------------------------------------------------------------------------------------- -bool ModelLoader::create(const tue::config::DataConstPointer& data, const UUID& id_opt, const UUID& parent_id, - UpdateRequest& req, std::stringstream& error, const std::string& model_path, +bool ModelLoader::create(const tue::config::DataConstPointer& data, + const UUID& id_opt, + const UUID& parent_id, + UpdateRequest& req, + std::stringstream& error, + const std::string& model_path, const geo::Pose3D& pose_offset) { tue::config::Reader r(data); @@ -335,7 +349,6 @@ bool ModelLoader::create(const tue::config::DataConstPointer& data, const UUID& if (r.hasGroup("sdf")) return createSDF(r.data(), parent_id, pose_offset, id_opt, boost::shared_ptr(), req, error); - // Get Id UUID id; std::string id_str; @@ -360,7 +373,7 @@ bool ModelLoader::create(const tue::config::DataConstPointer& data, const UUID& if (r.value("type", type, tue::config::OPTIONAL)) { std::vector types; - tue::config::DataConstPointer super_data = loadModelData(type, types, error); + tue::config::DataConstPointer const super_data = loadModelData(type, types, error); if (super_data.empty()) return false; @@ -372,8 +385,8 @@ bool ModelLoader::create(const tue::config::DataConstPointer& data, const UUID& r = tue::config::Reader(data_combined); types.push_back(type); - for(std::vector::const_iterator it = types.begin(); it != types.end(); ++it) - req.addType(id, *it); + for (const auto& type : types) + req.addType(id, type); } // Set type @@ -382,7 +395,7 @@ bool ModelLoader::create(const tue::config::DataConstPointer& data, const UUID& // Get pose geo::Pose3D pose = geo::Pose3D::identity(); if (!ed::models::readPose(r, pose)) - error << "[ed::models::create] No pose, while reading model: '" << id << "'" << std::endl; + error << "[ed::models::create] No pose, while reading model: '" << id << "'" << '\n'; pose = pose_offset * pose; @@ -400,13 +413,12 @@ bool ModelLoader::create(const tue::config::DataConstPointer& data, const UUID& r.endArray(); } - std::string shape_model_path = model_path; r.value("__model_path__", shape_model_path); // Set shape if (r.readGroup("shape")) { - geo::ShapePtr shape = loadShape(shape_model_path, r, shape_cache_, error); + geo::ShapePtr const shape = loadShape(shape_model_path, r, shape_cache_, error); if (shape) { req.setVisual(id, shape); @@ -430,11 +442,11 @@ bool ModelLoader::create(const tue::config::DataConstPointer& data, const UUID& geo::CompositeShapePtr shape; while (r.nextArrayItem()) { - geo::ShapePtr sub_shape = loadShape(shape_model_path, r, shape_cache_, error); + geo::ShapePtr const sub_shape = loadShape(shape_model_path, r, shape_cache_, error); if (sub_shape) { - if(!shape) - shape.reset(new geo::CompositeShape); + if (!shape) + shape = std::make_shared(); shape->addShape(*sub_shape, geo::Pose3D::identity()); } } @@ -463,14 +475,19 @@ bool ModelLoader::create(const tue::config::DataConstPointer& data, const UUID& return true; } -bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUID& parent_id, const geo::Pose3D& parent_pose, const UUID& id_override, - const boost::shared_ptr pose_override, UpdateRequest& req, std::stringstream& error) +bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, + const UUID& parent_id, + const geo::Pose3D& parent_pose, + const UUID& id_override, + const boost::shared_ptr& pose_override, + UpdateRequest& req, + std::stringstream& error) { tue::config::Reader r(data); r.readGroup("sdf"); // Just read the sdf element - bool sdf_world = r.readGroup("world"); + bool const sdf_world = r.readGroup("world"); bool sdf_model = false; if (!sdf_world) { @@ -480,17 +497,18 @@ bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUI if (!sdf_world && !sdf_model) { - error << "[ed::models::createSDF] Not a valid SDF model, because no 'world' or 'model' available: " << std::endl << data << std::endl; + error << "[ed::models::createSDF] Not a valid SDF model, because no 'world' or 'model' available: " << '\n' + << data << '\n'; return false; } if (r.nextArrayItem()) { - error << "[ed::models::createSDF] A model sdf file should only contain one model." << std::endl; + error << "[ed::models::createSDF] A model sdf file should only contain one model." << '\n'; return false; } - //ID + // ID UUID id; std::string id_str; if (!id_override.str().empty()) @@ -546,22 +564,22 @@ bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUI child_posePtr = ed::make_shared(child_pose); if (!r.value("uri", uri)) { - error << "No uri found for include in model: '" << id << "'." << std::endl << r.data() << std::endl; + error << "No uri found for include in model: '" << id << "'." << '\n' << r.data() << '\n'; return false; } - std::vector types; - tue::config::DataConstPointer child_data = loadSDFData(uri, error); + std::vector const types; + tue::config::DataConstPointer const child_data = loadSDFData(uri, error); if (!createSDF(child_data, id, pose, child_id, child_posePtr, req, error)) return false; } r.endArray(); // end array include } - // visual, collision & volumes - geo::CompositeShapePtr visual_composite, collision_composite; - std::map dummy_shape_cache; + geo::CompositeShapePtr visual_composite; + geo::CompositeShapePtr collision_composite; + std::map const dummy_shape_cache; if (r.readArray("link")) { while (r.nextArrayItem()) @@ -570,7 +588,7 @@ bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUI readPose(r, link_pose); if (r.readArray("visual")) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { readSDFGeometry(r, visual_composite, error, link_pose); } @@ -578,7 +596,7 @@ bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUI } if (r.readArray("collision")) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { readSDFGeometry(r, collision_composite, error, link_pose); } @@ -590,7 +608,7 @@ bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUI { if (r.readArray("virtual_volume")) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { readSDFGeometry(r, volume_composite, error, link_pose); } @@ -598,7 +616,7 @@ bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUI } if (volume_composite) req.addVolume(id, volume_name, volume_composite); - } + } } r.endArray(); // end array link } @@ -607,7 +625,7 @@ bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUI if (collision_composite) req.setCollision(id, collision_composite); - if(sdf_world) + if (sdf_world) r.endGroup(); // end group world else // sdf_model r.endArray(); // end array model @@ -615,7 +633,6 @@ bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUI return true; } -} // end namespace models - -} // end namespace ed +} // namespace ed::models +// end namespace ed diff --git a/src/models/shape_loader.cpp b/src/models/shape_loader.cpp index 6b7196dd..6196697d 100644 --- a/src/models/shape_loader.cpp +++ b/src/models/shape_loader.cpp @@ -1,27 +1,43 @@ -#include "shape_loader_private.h" #include "ed/models/shape_loader.h" +#include "shape_loader_private.h" #include "xml_shape_parser.h" -#include +#include +#include +#include +#include #include +#include #include +#include +#include #include #include // Heightmap generation #include "polypartition/polypartition.h" -#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include // string split #include -#include +#include +#include +#include +#include +#include -namespace ed -{ -namespace models +namespace ed::models { /** @@ -54,16 +70,16 @@ std::string parseURI(const std::string& uri, ModelOrFile& uri_type) std::string::size_type i = type.find(file_prefix); if (i != std::string::npos) { - uri_type = FILE; - type.erase(i, file_prefix.length()); - return type; + uri_type = FILE; + type.erase(i, file_prefix.length()); + return type; } i = uri.find(model_prefix); if (i != std::string::npos) { - uri_type = MODEL; - type.erase(i, model_prefix.length()); - return type; + uri_type = MODEL; + type.erase(i, model_prefix.length()); + return type; } return ""; } @@ -75,10 +91,10 @@ std::string parseURI(const std::string& uri, ModelOrFile& uri_type) * @param type subpath+filename incl. extension * @return full path or empty string in case not found */ -static std::string getUriPath(std::string type) +static std::string getUriPath(const std::string& type) { - static const char * mpath = ::getenv("GAZEBO_MODEL_PATH"); - static const char * rpath = ::getenv("GAZEBO_RESOURCE_PATH"); + static const char* mpath = ::getenv("GAZEBO_MODEL_PATH"); + static const char* rpath = ::getenv("GAZEBO_RESOURCE_PATH"); if (!mpath && !rpath) return ""; @@ -105,30 +121,27 @@ static std::string getUriPath(std::string type) file_paths.erase(unique(file_paths.begin(), file_paths.end()), file_paths.end()); } - ModelOrFile uri_type; - std::string parsed_uri = parseURI(type, uri_type); + std::string const parsed_uri = parseURI(type, uri_type); if (parsed_uri.empty()) return ""; - - std::vector* type_paths; + std::vector const* type_paths = nullptr; if (uri_type == MODEL) type_paths = &model_paths; else type_paths = &file_paths; - for(std::vector::const_iterator it = type_paths->cbegin(); it != type_paths->cend(); ++it) + for (const auto& type_path : *type_paths) { - tue::filesystem::Path file_path(*it + "/" + parsed_uri); - if (file_path.exists()) + std::filesystem::path const file_path(type_path + "/" + parsed_uri); + if (std::filesystem::exists(file_path)) return file_path.string(); } return ""; } - // ---------------------------------------------------------------------------------------------------- /** @@ -140,13 +153,17 @@ static std::string getUriPath(std::string type) * @param line_starts * @param contour_map */ -void findContours(const cv::Mat& image, const geo::Vec2i& p_start, int d_start, std::vector& points, - std::vector& line_starts, cv::Mat& contour_map) +static void findContours(const cv::Mat& image, + const geo::Vec2i& p_start, + int d_start, + std::vector& points, + std::vector& line_starts, + cv::Mat& contour_map) { - static int dx[4] = {1, 0, -1, 0 }; - static int dy[4] = {0, 1, 0, -1 }; + static int const dx[4] = {1, 0, -1, 0}; + static int const dy[4] = {0, 1, 0, -1}; - unsigned char v = image.at(p_start.y, p_start.x); + unsigned char const v = image.at(p_start.y, p_start.x); int d = d_start; // Current direction geo::Vec2i p = p_start; @@ -165,10 +182,10 @@ void findContours(const cv::Mat& image, const geo::Vec2i& p_start, int d_start, { switch (d) { - case 0: points.push_back(p - geo::Vec2i(1, 1)); break; - case 1: points.push_back(p - geo::Vec2i(0, 1)); break; - case 2: points.push_back(p); break; - case 3: points.push_back(p - geo::Vec2i(1, 0)); break; + case 0: points.push_back(p - geo::Vec2i(1, 1)); break; + case 1: points.push_back(p - geo::Vec2i(0, 1)); break; + case 2: points.push_back(p); break; + case 3: points.push_back(p - geo::Vec2i(1, 0)); break; } d = (d + 3) % 4; @@ -184,11 +201,10 @@ void findContours(const cv::Mat& image, const geo::Vec2i& p_start, int d_start, { switch (d) { - case 0: points.push_back(p - geo::Vec2i(0, 1)); break; - case 1: points.push_back(p); break; - case 2: points.push_back(p - geo::Vec2i(1, 0)); break; - case 3: points.push_back(p - geo::Vec2i(1, 1)); break; - + case 0: points.push_back(p - geo::Vec2i(0, 1)); break; + case 1: points.push_back(p); break; + case 2: points.push_back(p - geo::Vec2i(1, 0)); break; + case 3: points.push_back(p - geo::Vec2i(1, 1)); break; } d = (d + 1) % 4; @@ -210,11 +226,12 @@ void findContours(const cv::Mat& image, const geo::Vec2i& p_start, int d_start, * @param error errorstream * @return final mesh; or empty mesh in case of error */ -geo::ShapePtr getHeightMapShape(cv::Mat& image_orig, const geo::Vec3& pos, const geo::Vec3& size, const bool inverted, std::stringstream& error) +static geo::ShapePtr getHeightMapShape( + cv::Mat& image_orig, const geo::Vec3& pos, const geo::Vec3& size, const bool inverted, std::stringstream& error) { - double resolution_x = size.x/image_orig.cols; - double resolution_y = size.y/image_orig.rows; - double blockheight = size.z; + double const resolution_x = size.x / image_orig.cols; + double const resolution_y = size.y / image_orig.rows; + double const blockheight = size.z; // invert grayscale for SDF if (inverted) @@ -237,39 +254,40 @@ geo::ShapePtr getHeightMapShape(cv::Mat& image_orig, const geo::Vec3& pos, const geo::CompositeShapePtr shape(new geo::CompositeShape); - for(int y = 0; y < image.rows; ++y) + for (int y = 0; y < image.rows; ++y) { - for(int x = 0; x < image.cols; ++x) + for (int x = 0; x < image.cols; ++x) { - unsigned char v = image.at(y, x); + unsigned char const v = image.at(y, x); if (v < 255) { - std::vector points, line_starts; + std::vector points; + std::vector line_starts; findContours(image, geo::Vec2i(x, y), 0, points, line_starts, contour_map); - unsigned int num_points = (unsigned int) points.size(); + auto const num_points = static_cast(points.size()); if (num_points > 2) { geo::Mesh mesh; - double min_z = pos.z; - double max_z = pos.z + (double)(255 - v) / 255 * blockheight; + double const min_z = pos.z; + double const max_z = pos.z + (static_cast(255 - v) / 255 * blockheight); std::list testpolys; TPPLPoly poly; poly.Init(num_points); - for(unsigned int i = 0; i < num_points; ++i) + for (unsigned int i = 0; i < num_points; ++i) { poly[i].x = points[i].x; poly[i].y = points[i].y; // Convert to world coordinates - double wx = points[i].x * resolution_x + pos.x; - double wy = (image.rows - points[i].y - 2) * resolution_y + pos.y; + double const wx = (points[i].x * resolution_x) + pos.x; + double const wy = ((image.rows - points[i].y - 2) * resolution_y) + pos.y; vertex_index_map.at(points[i].y, points[i].x) = mesh.addPoint(geo::Vector3(wx, wy, min_z)); mesh.addPoint(geo::Vector3(wx, wy, max_z)); @@ -278,19 +296,19 @@ geo::ShapePtr getHeightMapShape(cv::Mat& image_orig, const geo::Vec3& pos, const testpolys.push_back(poly); // Calculate side triangles - for(unsigned int i = 0; i < num_points; ++i) + for (unsigned int i = 0; i < num_points; ++i) { - int j = (i + 1) % num_points; - mesh.addTriangle(i * 2, i * 2 + 1, j * 2); - mesh.addTriangle(i * 2 + 1, j * 2 + 1, j * 2); + int const j = (i + 1) % num_points; + mesh.addTriangle(i * 2, (i * 2) + 1, j * 2); + mesh.addTriangle((i * 2) + 1, (j * 2) + 1, j * 2); } - for(unsigned int i = 0; i < line_starts.size(); ++i) + for (unsigned int i = 0; i < line_starts.size(); ++i) { int x2 = line_starts[i].x; - int y2 = line_starts[i].y; + int const y2 = line_starts[i].y; - while(image.at(y2, x2) == v) + while (image.at(y2, x2) == v) ++x2; if (contour_map.at(y2, x2 - 1) == 0) @@ -305,28 +323,29 @@ geo::ShapePtr getHeightMapShape(cv::Mat& image_orig, const geo::Vec3& pos, const poly_hole.Init(hole_points.size()); poly_hole.SetHole(true); - for(unsigned int j = 0; j < hole_points.size(); ++j) + for (unsigned int j = 0; j < hole_points.size(); ++j) { poly_hole[j].x = hole_points[j].x; poly_hole[j].y = hole_points[j].y; // Convert to world coordinates - double wx = hole_points[j].x * resolution_x + pos.x; - double wy = (image.rows - hole_points[j].y - 2) * resolution_y + pos.y; + double const wx = (hole_points[j].x * resolution_x) + pos.x; + double const wy = ((image.rows - hole_points[j].y - 2) * resolution_y) + pos.y; - vertex_index_map.at(hole_points[j].y, hole_points[j].x) = mesh.addPoint(geo::Vector3(wx, wy, min_z)); + vertex_index_map.at(hole_points[j].y, hole_points[j].x) = + mesh.addPoint(geo::Vector3(wx, wy, min_z)); mesh.addPoint(geo::Vector3(wx, wy, max_z)); } testpolys.push_back(poly_hole); // Calculate side triangles - for(unsigned int j = 0; j < hole_points.size(); ++j) + for (unsigned int j = 0; j < hole_points.size(); ++j) { const geo::Vec2i& hp1 = hole_points[j]; const geo::Vec2i& hp2 = hole_points[(j + 1) % hole_points.size()]; - int i1 = vertex_index_map.at(hp1.y, hp1.x); - int i2 = vertex_index_map.at(hp2.y, hp2.x); + int const i1 = vertex_index_map.at(hp1.y, hp1.x); + int const i2 = vertex_index_map.at(hp2.y, hp2.x); mesh.addTriangle(i1, i1 + 1, i2); mesh.addTriangle(i2, i1 + 1, i2 + 1); @@ -340,17 +359,17 @@ geo::ShapePtr getHeightMapShape(cv::Mat& image_orig, const geo::Vec3& pos, const if (!pp.Triangulate_EC(&testpolys, &result)) { - error << "[ED::MODELS::LOADSHAPE] Error while creating heightmap: could not triangulate polygon." << std::endl; - return geo::ShapePtr(); + error + << "[ED::MODELS::LOADSHAPE] Error while creating heightmap: could not triangulate polygon." + << '\n'; + return {}; } - for(std::list::iterator it = result.begin(); it != result.end(); ++it) + for (auto& cp : result) { - TPPLPoly& cp = *it; - - int i1 = vertex_index_map.at(cp[0].y, cp[0].x) + 1; - int i2 = vertex_index_map.at(cp[1].y, cp[1].x) + 1; - int i3 = vertex_index_map.at(cp[2].y, cp[2].x) + 1; + int const i1 = vertex_index_map.at(cp[0].y, cp[0].x) + 1; + int const i2 = vertex_index_map.at(cp[1].y, cp[1].x) + 1; + int const i3 = vertex_index_map.at(cp[2].y, cp[2].x) + 1; mesh.addTriangle(i1, i3, i2); } @@ -380,42 +399,50 @@ geo::ShapePtr getHeightMapShape(cv::Mat& image_orig, const geo::Vec3& pos, const * @param errorerrorstream * @return final mesh; or empty mesh in case of error */ -geo::ShapePtr getHeightMapShape(const std::string& image_filename, const geo::Vec3& pos, const geo::Vec3& size, - const bool inverted, std::stringstream& error) +static geo::ShapePtr getHeightMapShape(const std::string& image_filename, + const geo::Vec3& pos, + const geo::Vec3& size, + const bool inverted, + std::stringstream& error) { - cv::Mat image_orig = cv::imread(image_filename, cv::IMREAD_GRAYSCALE); // Read the file + cv::Mat image_orig = cv::imread(image_filename, cv::IMREAD_GRAYSCALE); // Read the file if (!image_orig.data) { - error << "[ED::MODELS::LOADSHAPE] Error while loading heightmap '" << image_filename << "'. Image could not be loaded." << std::endl; - return geo::ShapePtr(); + error << "[ED::MODELS::LOADSHAPE] Error while loading heightmap '" << image_filename + << "'. Image could not be loaded." << '\n'; + return {}; } return getHeightMapShape(image_orig, pos, size, inverted, error); } - // ---------------------------------------------------------------------------------------------------- -geo::ShapePtr getHeightMapShape(const std::string& image_filename, const geo::Vec3& pos, const double blockheight, - const double resolution_x, const double resolution_y, const bool inverted, std::stringstream& error) +geo::ShapePtr getHeightMapShape(const std::string& image_filename, + const geo::Vec3& pos, + const double blockheight, + const double resolution_x, + const double resolution_y, + const bool inverted, + std::stringstream& error) { - cv::Mat image_orig = cv::imread(image_filename, cv::IMREAD_GRAYSCALE); // Read the file + cv::Mat image_orig = cv::imread(image_filename, cv::IMREAD_GRAYSCALE); // Read the file if (!image_orig.data) { - error << "[ED::MODELS::LOADSHAPE] Error while loading heightmap '" << image_filename << "'. Image could not be loaded." << std::endl; - return geo::ShapePtr(); + error << "[ED::MODELS::LOADSHAPE] Error while loading heightmap '" << image_filename + << "'. Image could not be loaded." << '\n'; + return {}; } - double size_x = resolution_x * image_orig.cols; - double size_y = resolution_y * image_orig.rows; - geo::Vec3 size(size_x, size_y, blockheight); + double const size_x = resolution_x * image_orig.cols; + double const size_y = resolution_y * image_orig.rows; + geo::Vec3 const size(size_x, size_y, blockheight); return getHeightMapShape(image_orig, pos, size, inverted, error); } - // ---------------------------------------------------------------------------------------------------- /** @@ -425,40 +452,51 @@ geo::ShapePtr getHeightMapShape(const std::string& image_filename, const geo::Ve * @param error errorstream * @return final mesh; or empty mesh in case of error */ -geo::ShapePtr getHeightMapShape(const std::string& image_filename, tue::config::Reader cfg, std::stringstream& error) +static geo::ShapePtr +getHeightMapShape(const std::string& image_filename, const tue::config::Reader& cfg, std::stringstream& error) { - double resolution, origin_x, origin_y, origin_z, blockheight; - if (!(cfg.value("origin_x", origin_x) && - cfg.value("origin_y", origin_y) && - cfg.value("origin_z", origin_z) && - cfg.value("resolution", resolution) && - cfg.value("blockheight", blockheight))) + double resolution = NAN; + double origin_x = NAN; + double origin_y = NAN; + double origin_z = NAN; + double blockheight = NAN; + if (!(cfg.value("origin_x", origin_x) && cfg.value("origin_y", origin_y) && cfg.value("origin_z", origin_z) && + cfg.value("resolution", resolution) && cfg.value("blockheight", blockheight))) { error << "[ED::MODELS::LOADSHAPE] Error while loading heightmap parameters at '" << image_filename - << "'. Required shape parameters: resolution, origin_x, origin_y, origin_z, blockheight" << std::endl; - return geo::ShapePtr(); + << "'. Required shape parameters: resolution, origin_x, origin_y, origin_z, blockheight" << '\n'; + return {}; } int inverted = 0; cfg.value("inverted", inverted); - return getHeightMapShape(image_filename, geo::Vec3(origin_x, origin_y, origin_z), blockheight, resolution, resolution, - static_cast(inverted), error); + return getHeightMapShape(image_filename, + geo::Vec3(origin_x, origin_y, origin_z), + blockheight, + resolution, + resolution, + static_cast(inverted), + error); } // ---------------------------------------------------------------------------------------------------- -void createPolygon(geo::Shape& shape, const std::vector& points, double height, std::stringstream& error, bool create_bottom) +void createPolygon(geo::Shape& shape, + const std::vector& points, + double height, + std::stringstream& error, + bool create_bottom) { TPPLPoly poly; - poly.Init((unsigned int) points.size()); + poly.Init(static_cast(points.size())); - double min_z = -height / 2; - double max_z = height / 2; + double const min_z = -height / 2; + double const max_z = height / 2; geo::Mesh mesh; - for(unsigned int i = 0; i < points.size(); ++i) + for (unsigned int i = 0; i < points.size(); ++i) { poly[i].x = points[i].x; poly[i].y = points[i].y; @@ -468,11 +506,11 @@ void createPolygon(geo::Shape& shape, const std::vector& points, doub } // Add side triangles - for(unsigned int i = 0; i < points.size(); ++i) + for (unsigned int i = 0; i < points.size(); ++i) { - int j = (i + 1) % points.size(); - mesh.addTriangle(i * 2, j * 2, i * 2 + 1); - mesh.addTriangle(i * 2 + 1, j * 2, j * 2 + 1); + int const j = (i + 1) % points.size(); + mesh.addTriangle(i * 2, j * 2, (i * 2) + 1); + mesh.addTriangle((i * 2) + 1, j * 2, (j * 2) + 1); } std::list polys; @@ -483,24 +521,22 @@ void createPolygon(geo::Shape& shape, const std::vector& points, doub if (!pp.Triangulate_EC(&polys, &result)) { - error << "[ED::MODELS::LOADSHAPE](createPolygon) TRIANGULATION FAILED" << std::endl; + error << "[ED::MODELS::LOADSHAPE](createPolygon) TRIANGULATION FAILED" << '\n'; return; } - for(std::list::iterator it = result.begin(); it != result.end(); ++it) + for (auto& cp : result) { - TPPLPoly& cp = *it; - - int i1 = mesh.addPoint(cp[0].x, cp[0].y, max_z); - int i2 = mesh.addPoint(cp[1].x, cp[1].y, max_z); - int i3 = mesh.addPoint(cp[2].x, cp[2].y, max_z); + int const i1 = mesh.addPoint(cp[0].x, cp[0].y, max_z); + int const i2 = mesh.addPoint(cp[1].x, cp[1].y, max_z); + int const i3 = mesh.addPoint(cp[2].x, cp[2].y, max_z); mesh.addTriangle(i1, i2, i3); if (create_bottom) { - int i1 = mesh.addPoint(cp[0].x, cp[0].y, min_z); - int i2 = mesh.addPoint(cp[1].x, cp[1].y, min_z); - int i3 = mesh.addPoint(cp[2].x, cp[2].y, min_z); + int const i1 = mesh.addPoint(cp[0].x, cp[0].y, min_z); + int const i2 = mesh.addPoint(cp[1].x, cp[1].y, min_z); + int const i3 = mesh.addPoint(cp[2].x, cp[2].y, min_z); mesh.addTriangle(i1, i3, i2); } } @@ -518,7 +554,8 @@ void createPolygon(geo::Shape& shape, const std::vector& points, doub * @param v filled Vec3 vector * @param pos_req RequiredOrOptional */ -void readVec3(tue::config::Reader& cfg, geo::Vec3& v, tue::config::RequiredOrOptional pos_req = tue::config::REQUIRED) +static void +readVec3(tue::config::Reader& cfg, geo::Vec3& v, tue::config::RequiredOrOptional pos_req = tue::config::REQUIRED) { cfg.value("x", v.x, pos_req); cfg.value("y", v.y, pos_req); @@ -535,7 +572,10 @@ void readVec3(tue::config::Reader& cfg, geo::Vec3& v, tue::config::RequiredOrOpt * @param pos_req RequiredOrOptional * @return indicates succes */ -bool readVec3Group(tue::config::Reader& cfg, geo::Vec3& v, const std::string& vector_name, tue::config::RequiredOrOptional /*pos_req*/ = tue::config::REQUIRED) +static bool readVec3Group(tue::config::Reader& cfg, + geo::Vec3& v, + const std::string& vector_name, + tue::config::RequiredOrOptional /*pos_req*/ = tue::config::REQUIRED) { std::string vector_string; if (cfg.readGroup(vector_name)) @@ -558,20 +598,25 @@ bool readVec3Group(tue::config::Reader& cfg, geo::Vec3& v, const std::string& ve // ---------------------------------------------------------------------------------------------------- -bool readPose(tue::config::Reader& cfg, geo::Pose3D& pose, tue::config::RequiredOrOptional pos_req, tue::config::RequiredOrOptional rot_req) +bool readPose(tue::config::Reader& cfg, + geo::Pose3D& pose, + tue::config::RequiredOrOptional pos_req, + tue::config::RequiredOrOptional rot_req) { - double roll = 0, pitch = 0, yaw = 0; - std::string pose_string = ""; //sdf pose will be a string + double roll = 0; + double pitch = 0; + double yaw = 0; + std::string pose_string; // sdf pose will be a string if (cfg.readGroup("pose")) { readVec3(cfg, pose.t, pos_req); - cfg.value("X", roll, rot_req); + cfg.value("X", roll, rot_req); cfg.value("Y", pitch, rot_req); - cfg.value("Z", yaw, rot_req); - cfg.value("roll", roll, rot_req); + cfg.value("Z", yaw, rot_req); + cfg.value("roll", roll, rot_req); cfg.value("pitch", pitch, rot_req); - cfg.value("yaw", yaw, rot_req); + cfg.value("yaw", yaw, rot_req); cfg.endGroup(); } @@ -618,8 +663,10 @@ bool readPose(tue::config::Reader& cfg, geo::Pose3D& pose, tue::config::Required // ---------------------------------------------------------------------------------------------------- -geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, - std::map& shape_cache, std::stringstream& error) +geo::ShapePtr loadShape(const std::string& model_path, + tue::config::Reader cfg, + std::map& shape_cache, + std::stringstream& error) { geo::ShapePtr shape; geo::Pose3D pose = geo::Pose3D::identity(); @@ -633,7 +680,7 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, return shape; } - tue::filesystem::Path shape_path; + std::filesystem::path shape_path; if (model_path.empty() || path[0] == '/') shape_path = path; @@ -641,21 +688,21 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, shape_path = model_path + "/" + path; // Check cache first - std::map::const_iterator it = shape_cache.find(shape_path.string()); + auto const it = shape_cache.find(shape_path.string()); if (it != shape_cache.end()) return it->second; - if (shape_path.exists()) + if (std::filesystem::exists(shape_path)) { - std::string xt = shape_path.extension(); + std::string const xt = shape_path.extension().string(); if (xt == ".pgm" || xt == ".png") { shape = getHeightMapShape(shape_path.string(), cfg, error); } else if (xt == ".geo") { - geo::serialization::registerDeserializer(); - shape = geo::serialization::fromFile(shape_path.string()); + geo::Serialization::registerDeserializer(); + shape = geo::Serialization::fromFile(shape_path.string()); } else if (xt == ".3ds" || xt == ".stl" || xt == ".dae") { @@ -668,32 +715,36 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, } if (!shape) - error << "[ED::MODELS::LOADSHAPE] Error while loading shape at " << shape_path.string() << std::endl; + error << "[ED::MODELS::LOADSHAPE] Error while loading shape at " << shape_path.string() << '\n'; else // Add to cache shape_cache[shape_path.string()] = shape; } else { - error << "[ED::MODELS::LOADSHAPE] Error while loading shape at " << shape_path.string() << " ; file does not exist" << std::endl; + error << "[ED::MODELS::LOADSHAPE] Error while loading shape at " << shape_path.string() + << " ; file does not exist" << '\n'; } } else if (cfg.readGroup("box")) // SDF AND ED YAML { - geo::Vec3 min, max, size; + geo::Vec3 min; + geo::Vec3 max; + geo::Vec3 size; if (readVec3Group(cfg, min, "min")) { if (readVec3Group(cfg, max, "max")) { - shape.reset(new geo::Box(min, max)); + shape = std::make_shared(min, max); } else { - error << "[ED::MODELS::LOADSHAPE] Error while loading shape: box must contain 'min' and 'max' (only 'min' specified)"; + error << "[ED::MODELS::LOADSHAPE] Error while loading shape: box must contain 'min' and 'max' (only " + "'min' specified)"; } } else if (readVec3Group(cfg, size, "size")) - shape.reset(new geo::Box(-0.5 * size, 0.5 * size)); + shape = std::make_shared(-0.5 * size, 0.5 * size); else { error << "[ED::MODELS::LOADSHAPE] Error while loading shape: box must contain 'min' and 'max' or 'size'."; @@ -707,11 +758,13 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, int num_points = 12; cfg.value("num_points", num_points, tue::config::OPTIONAL); - double radius = 0, height = 0; - if (cfg.value("radius", radius) && (cfg.value("height", height) || cfg.value("length", height))) // length is used in SDF + double radius = 0; + double height = 0; + if (cfg.value("radius", radius) && + (cfg.value("height", height) || cfg.value("length", height))) // length is used in SDF { - shape.reset(new geo::Shape()); + shape = std::make_shared(); createCylinder(*shape, radius, height, num_points); } @@ -722,9 +775,9 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, std::vector points; if (cfg.readArray("points", tue::config::REQUIRED) || cfg.readArray("point", tue::config::REQUIRED)) { - while(cfg.nextArrayItem()) + while (cfg.nextArrayItem()) { - points.push_back(geo::Vec2()); + points.emplace_back(); geo::Vec2& p = points.back(); cfg.value("x", p.x); cfg.value("y", p.y); @@ -732,10 +785,10 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, cfg.endArray(); } - double height; + double height = NAN; if (cfg.value("height", height)) { - shape.reset(new geo::Shape()); + shape = std::make_shared(); createPolygon(*shape, points, height, error, true); } @@ -750,7 +803,7 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, { std::string point_string; cfg.value("point", point_string); - points.push_back(geo::Vec2()); + points.emplace_back(); geo::Vec2& p = points.back(); std::vector point_vector = split(point_string, ' '); p.x = std::stod(point_vector[0]); @@ -759,10 +812,10 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, cfg.endArray(); } - double height; + double height = NAN; if (cfg.value("height", height)) { - shape.reset(new geo::Shape()); + shape = std::make_shared(); createPolygon(*shape, points, height, error, true); } @@ -778,55 +831,57 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, if (cfg.value("scale", scale_str)) { std::vector scale_vector = split(scale_str, ' '); - if(scale_vector.size() != 3) + if (scale_vector.size() != 3) { - error << "[ED::MODELS::LOADSHAPE] Mesh scale: '" << scale_str << "' should have 3 members." << std::endl; + error << "[ED::MODELS::LOADSHAPE] Mesh scale: '" << scale_str << "' should have 3 members." << '\n'; return shape; } scale.x = std::stod(scale_vector[0]); scale.y = std::stod(scale_vector[1]); scale.z = std::stod(scale_vector[2]); } - tue::filesystem::Path mesh_path = getUriPath(uri_path); - if (mesh_path.exists()) + std::filesystem::path const mesh_path = getUriPath(uri_path); + if (std::filesystem::exists(mesh_path)) shape = geo::io::readMeshFile(mesh_path.string(), scale); else - error << "[ED::MODELS::LOADSHAPE] Mesh File: '" << mesh_path.string() << "' doesn't exist." << std::endl; + error << "[ED::MODELS::LOADSHAPE] Mesh File: '" << mesh_path.string() << "' doesn't exist." << '\n'; } else - error << "[ED::MODELS::LOADSHAPE] No uri found for mesh." << std::endl; + error << "[ED::MODELS::LOADSHAPE] No uri found for mesh." << '\n'; std::string dummy; if (cfg.value("submesh", dummy)) - error << "[ED::MODELS::LOADSHAPE] 'submesh' of mesh is not supported by ED " << std::endl; + error << "[ED::MODELS::LOADSHAPE] 'submesh' of mesh is not supported by ED " << '\n'; cfg.endGroup(); } else if (cfg.readGroup("heightmap")) // SDF AND ED YAML { std::string image_filename; - double height, resolution; + double height = NAN; + double resolution = NAN; geo::Vec3 size; if ((cfg.value("image", image_filename) && !image_filename.empty() && cfg.value("resolution", resolution) && cfg.value("height", height))) // ED YAML ONLY { - std::string image_filename_full = image_filename; -// if (image_filename[0] == '/') -// image_filename_full = image_filename; -// else -// image_filename_full = model_path + "/" + image_filename; + const std::string& image_filename_full = image_filename; + // if (image_filename[0] == '/') + // image_filename_full = image_filename; + // else + // image_filename_full = model_path + "/" + image_filename; - shape = getHeightMapShape(image_filename_full, geo::Vec3(0, 0, 0), height, resolution, resolution, false, error); + shape = getHeightMapShape( + image_filename_full, geo::Vec3(0, 0, 0), height, resolution, resolution, false, error); readPose(cfg, pose); } - else if(cfg.value("uri", image_filename) && readVec3Group(cfg, size, "size")) // SDF ONLY + else if (cfg.value("uri", image_filename) && readVec3Group(cfg, size, "size")) // SDF ONLY { image_filename = getUriPath(image_filename); // Center is in the middle. - geo::Vec3 pos = -size/2; + geo::Vec3 pos = -size / 2; pos.z = 0; shape = getHeightMapShape(image_filename, pos, size, true, error); @@ -834,27 +889,29 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, } else { - error << "[ED::MODELS::LOADSHAPE] Error while loading shape: heightmap must contain 'image', 'resolution' and 'height'." << std::endl; + error << "[ED::MODELS::LOADSHAPE] Error while loading shape: heightmap must contain 'image', 'resolution' " + "and 'height'." + << '\n'; } cfg.endGroup(); } else if (cfg.readGroup("sphere")) // SDF { - double radius; + double radius = NAN; if (!cfg.value("radius", radius)) - error << "[ED::MODELS::LOADSHAPE] Error while loading shape: sphere must contain 'radius'." << std::endl; - int recursion_level = 1; - shape.reset(new geo::Shape); + error << "[ED::MODELS::LOADSHAPE] Error while loading shape: sphere must contain 'radius'." << '\n'; + int const recursion_level = 1; + shape = std::make_shared(); createSphere(*shape, radius, recursion_level); cfg.endGroup(); } else if (cfg.readArray("compound") || cfg.readArray("group")) // ED YAML ONLY { - geo::CompositeShapePtr composite(new geo::CompositeShape); - while(cfg.nextArrayItem()) + geo::CompositeShapePtr const composite(new geo::CompositeShape); + while (cfg.nextArrayItem()) { std::map dummy_shape_cache; - geo::ShapePtr sub_shape = loadShape(model_path, cfg, dummy_shape_cache, error); + geo::ShapePtr const sub_shape = loadShape(model_path, cfg, dummy_shape_cache, error); composite->addShape(*sub_shape, geo::Pose3D::identity()); } cfg.endArray(); @@ -863,7 +920,7 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, } else { - error << "[ED::MODELS::LOADSHAPE] Error while loading shape with data:" << std::endl << cfg.data() << std::endl; + error << "[ED::MODELS::LOADSHAPE] Error while loading shape with data:" << '\n' << cfg.data() << '\n'; } // Extra pose is only allowed in ED yaml. @@ -871,7 +928,7 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, if (shape && pose != geo::Pose3D::identity()) { // Transform shape according to pose - geo::ShapePtr shape_tr(new geo::Shape()); + geo::ShapePtr const shape_tr(new geo::Shape()); shape_tr->setMesh(shape->getMesh().getTransformed(pose)); shape = shape_tr; } @@ -886,20 +943,20 @@ void createCylinder(geo::Shape& shape, double radius, double height, int num_cor geo::Mesh mesh; // Calculate vertices - for(int i = 0; i < num_corners; ++i) + for (int i = 0; i < num_corners; ++i) { - double a = 2 * M_PI * i / num_corners; - double x = sin(a) * radius; - double y = cos(a) * radius; + double const a = 2 * M_PI * i / num_corners; + double const x = sin(a) * radius; + double const y = cos(a) * radius; mesh.addPoint(x, y, -height / 2); - mesh.addPoint(x, y, height / 2); + mesh.addPoint(x, y, height / 2); } // Calculate top and bottom triangles - for(int i = 1; i < num_corners - 1; ++i) + for (int i = 1; i < num_corners - 1; ++i) { - int i2 = 2 * i; + int const i2 = 2 * i; // bottom mesh.addTriangle(0, i2, i2 + 2); @@ -909,11 +966,11 @@ void createCylinder(geo::Shape& shape, double radius, double height, int num_cor } // Calculate side triangles - for(int i = 0; i < num_corners; ++i) + for (int i = 0; i < num_corners; ++i) { - int j = (i + 1) % num_corners; - mesh.addTriangle(i * 2, i * 2 + 1, j * 2); - mesh.addTriangle(i * 2 + 1, j * 2 + 1, j * 2); + int const j = (i + 1) % num_corners; + mesh.addTriangle(i * 2, (i * 2) + 1, j * 2); + mesh.addTriangle((i * 2) + 1, (j * 2) + 1, j * 2); } shape.setMesh(mesh); @@ -923,29 +980,29 @@ void createCylinder(geo::Shape& shape, double radius, double height, int num_cor uint getMiddlePoint(geo::Mesh& mesh, uint i1, uint i2, std::map cache, double radius) { - // first check if we have it already - bool firstIsSmaller = i1 < i2; - unsigned long smallerIndex = firstIsSmaller ? i1 : i2; - unsigned long greaterIndex = firstIsSmaller ? i2 : i1; - unsigned long key = (smallerIndex << 32) + greaterIndex; - - std::map::const_iterator it = cache.find(key); - if (it != cache.end()) - return it->second; - - // not in cache, calculate it - const std::vector& points = mesh.getPoints(); - geo::Vec3 p1 = points[i1]; - geo::Vec3 p2 = points[i2]; - geo::Vec3 p3((p1+p2)/2); - p3 = p3.normalized() * radius; - - // add vertex makes sure point is on unit sphere - uint i3 = mesh.addPoint(p3); - - // store it, return index - cache.insert(std::pair(key, i3)); - return i3; + // first check if we have it already + bool const firstIsSmaller = i1 < i2; + unsigned long const smallerIndex = firstIsSmaller ? i1 : i2; + unsigned long const greaterIndex = firstIsSmaller ? i2 : i1; + unsigned long const key = (smallerIndex << 32) + greaterIndex; + + auto const it = cache.find(key); + if (it != cache.end()) + return it->second; + + // not in cache, calculate it + const std::vector& points = mesh.getPoints(); + geo::Vec3 const p1 = points[i1]; + geo::Vec3 const p2 = points[i2]; + geo::Vec3 p3((p1 + p2) / 2); + p3 = p3.normalized() * radius; + + // add vertex makes sure point is on unit sphere + uint const i3 = mesh.addPoint(p3); + + // store it, return index + cache.insert(std::pair(key, i3)); + return i3; } // ---------------------------------------------------------------------------------------------------- @@ -955,22 +1012,22 @@ void createSphere(geo::Shape& shape, double radius, uint recursion_level) geo::Mesh mesh; // create 12 vertices of a icosahedron - double t = (1.0 + sqrt(5.0)) / 2.0; + double const t = (1.0 + sqrt(5.0)) / 2.0; - mesh.addPoint(geo::Vec3(-1, t, 0).normalized()*radius); - mesh.addPoint(geo::Vec3( 1, t, 0).normalized()*radius); - mesh.addPoint(geo::Vec3(-1, -t, 0).normalized()*radius); - mesh.addPoint(geo::Vec3( 1, -t, 0).normalized()*radius); + mesh.addPoint(geo::Vec3(-1, t, 0).normalized() * radius); + mesh.addPoint(geo::Vec3(1, t, 0).normalized() * radius); + mesh.addPoint(geo::Vec3(-1, -t, 0).normalized() * radius); + mesh.addPoint(geo::Vec3(1, -t, 0).normalized() * radius); - mesh.addPoint(geo::Vec3( 0, -1, t).normalized()*radius); - mesh.addPoint(geo::Vec3( 0, 1, t).normalized()*radius); - mesh.addPoint(geo::Vec3( 0, -1, -t).normalized()*radius); - mesh.addPoint(geo::Vec3( 0, 1, -t).normalized()*radius); + mesh.addPoint(geo::Vec3(0, -1, t).normalized() * radius); + mesh.addPoint(geo::Vec3(0, 1, t).normalized() * radius); + mesh.addPoint(geo::Vec3(0, -1, -t).normalized() * radius); + mesh.addPoint(geo::Vec3(0, 1, -t).normalized() * radius); - mesh.addPoint(geo::Vec3( t, 0, -1).normalized()*radius); - mesh.addPoint(geo::Vec3( t, 0, 1).normalized()*radius); - mesh.addPoint(geo::Vec3(-t, 0, -1).normalized()*radius); - mesh.addPoint(geo::Vec3(-t, 0, 1).normalized()*radius); + mesh.addPoint(geo::Vec3(t, 0, -1).normalized() * radius); + mesh.addPoint(geo::Vec3(t, 0, 1).normalized() * radius); + mesh.addPoint(geo::Vec3(-t, 0, -1).normalized() * radius); + mesh.addPoint(geo::Vec3(-t, 0, 1).normalized() * radius); // create 20 triangles of the icosahedron // 5 faces around point 0 @@ -1004,23 +1061,23 @@ void createSphere(geo::Shape& shape, double radius, uint recursion_level) for (uint i = 0; i < recursion_level; i++) { geo::Mesh mesh2; - std::map cache; + std::map const cache; const std::vector& points = mesh.getPoints(); - for (std::vector::const_iterator it = points.begin(); it != points.end(); ++it) - mesh2.addPoint(*it); + for (const auto& point : points) + mesh2.addPoint(point); const std::vector& triangleIs = mesh.getTriangleIs(); - for (std::vector::const_iterator it = triangleIs.begin(); it != triangleIs.end(); ++it) + for (auto triangleI : triangleIs) { // replace triangle by 4 triangles - uint a = getMiddlePoint(mesh2, it->i1_, it->i2_, cache, radius); - uint b = getMiddlePoint(mesh2, it->i2_, it->i3_, cache, radius); - uint c = getMiddlePoint(mesh2, it->i3_, it->i1_, cache, radius); + uint const a = getMiddlePoint(mesh2, triangleI.i1_, triangleI.i2_, cache, radius); + uint const b = getMiddlePoint(mesh2, triangleI.i2_, triangleI.i3_, cache, radius); + uint const c = getMiddlePoint(mesh2, triangleI.i3_, triangleI.i1_, cache, radius); - mesh2.addTriangle(it->i1_, a, c); - mesh2.addTriangle(it->i2_, b, a); - mesh2.addTriangle(it->i3_, c, b); + mesh2.addTriangle(triangleI.i1_, a, c); + mesh2.addTriangle(triangleI.i2_, b, a); + mesh2.addTriangle(triangleI.i3_, c, b); mesh2.addTriangle(a, b, c); } mesh = mesh2; @@ -1030,6 +1087,6 @@ void createSphere(geo::Shape& shape, double radius, uint recursion_level) // ---------------------------------------------------------------------------------------------------- -} // end namespace models +} // namespace ed::models -} // end namespace ed +// end namespace ed diff --git a/src/models/shape_loader_private.h b/src/models/shape_loader_private.h index 1a1b2c63..54855d62 100644 --- a/src/models/shape_loader_private.h +++ b/src/models/shape_loader_private.h @@ -1,22 +1,19 @@ #ifndef ED_MODELS_SHAPE_LOADER_PRIVATE_H_ #define ED_MODELS_SHAPE_LOADER_PRIVATE_H_ -// Some functions are moved to the include folder. So these can be included in other packages. There has been no need for the remaining -// functions in this file to be used in other packages. Therefore, these are kept here. But there is no reason for the functions not to -// be moved, if needed to be include somewhere else. +// Some functions are moved to the include folder. So these can be included in other packages. There has been no need +// for the remaining functions in this file to be used in other packages. Therefore, these are kept here. But there is +// no reason for the functions not to be moved, if needed to be include somewhere else. #include #include #include +#include #include #include -#include -namespace ed -{ - -namespace models +namespace ed::models { /** @@ -46,8 +43,10 @@ std::string parseURI(const std::string& uri, ModelOrFile& uri_type); * @param error errorstream * @return final mesh; or empty mesh in case of error */ -geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, - std::map& shape_cache, std::stringstream& error); +geo::ShapePtr loadShape(const std::string& model_path, + tue::config::Reader cfg, + std::map& shape_cache, + std::stringstream& error); /** * @brief getHeightMapShape convert grayscale image in a heigtmap mesh @@ -60,8 +59,13 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, * @param errorerrorstream * @return final mesh; or empty mesh in case of error */ -geo::ShapePtr getHeightMapShape(const std::string& image_filename, const geo::Vec3& pos, const double blockheight, - const double resolution_x, const double resolution_y, const bool inverted, std::stringstream& error); +geo::ShapePtr getHeightMapShape(const std::string& image_filename, + const geo::Vec3& pos, + const double blockheight, + const double resolution_x, + const double resolution_y, + const bool inverted, + std::stringstream& error); /** * @brief readPose read pose into Pose3D. Both ED yaml and SDF. Also reads pos(position) of SDF. @@ -71,7 +75,8 @@ geo::ShapePtr getHeightMapShape(const std::string& image_filename, const geo::Ve * @param rot_req rotation RequiredOrOptional * @return indicates succes */ -bool readPose(tue::config::Reader& cfg, geo::Pose3D& pose, +bool readPose(tue::config::Reader& cfg, + geo::Pose3D& pose, tue::config::RequiredOrOptional pos_req = tue::config::REQUIRED, tue::config::RequiredOrOptional rot_req = tue::config::OPTIONAL); @@ -83,10 +88,12 @@ bool readPose(tue::config::Reader& cfg, geo::Pose3D& pose, * @param error error stream * @param create_bottom false: open bottom; true: closed bottom */ -void createPolygon(geo::Shape& shape, const std::vector& points, double height, std::stringstream& error, bool create_bottom = true); - -} // end models namespace +void createPolygon(geo::Shape& shape, + const std::vector& points, + double height, + std::stringstream& error, + bool create_bottom = true); -} // end ed namespace +} // namespace ed::models #endif diff --git a/src/models/xml_shape_parser.cpp b/src/models/xml_shape_parser.cpp index 36ef19c6..54bc0fb1 100644 --- a/src/models/xml_shape_parser.cpp +++ b/src/models/xml_shape_parser.cpp @@ -1,25 +1,28 @@ #include "xml_shape_parser.h" -#include +#include #include +#include +#include #include -#include -#include +#include #include +#include +#include // ---------------------------------------------------------------------------------------------------- -std::vector parseArray(const tinyxml2::XMLElement* xml_elem) +static std::vector parseArray(const tinyxml2::XMLElement* xml_elem) { - std::string txt = xml_elem->GetText(); + std::string const txt = xml_elem->GetText(); std::vector v; std::string word; std::stringstream stream(txt); - while(getline(stream, word, ' ')) + while (getline(stream, word, ' ')) { double d = 0; std::istringstream istr(word); @@ -41,17 +44,18 @@ geo::ShapePtr parseXMLShape(const std::string& filename, std::string& error) if (doc.Error()) { - s_error << "While parsing '" << filename << "': " << std::endl << std::endl - << doc.ErrorStr() << " at line " << doc.ErrorLineNum() << std::endl; + s_error << "While parsing '" << filename << "': " << '\n' + << '\n' + << doc.ErrorStr() << " at line " << doc.ErrorLineNum() << '\n'; error = s_error.str(); - return geo::ShapePtr(); + return {}; } const tinyxml2::XMLElement* model_xml = doc.FirstChildElement("model"); if (!model_xml) { - s_error << "Could not find 'model' element" << std::endl; - return geo::ShapePtr(new geo::Shape()); + s_error << "Could not find 'model' element" << '\n'; + return std::make_shared(); } geo::CompositeShapePtr shape(new geo::CompositeShape); @@ -86,8 +90,9 @@ geo::ShapePtr parseXMLShape(const std::string& filename, std::string& error) if (size_xml) size = parseArray(size_xml); - std::string shape_type = shape_xml->Value(); - if (shape_type == "box") { + std::string const shape_type = shape_xml->Value(); + if (shape_type == "box") + { const tinyxml2::XMLElement* min_xml = shape_xml->FirstChildElement("min"); const tinyxml2::XMLElement* max_xml = shape_xml->FirstChildElement("max"); @@ -98,22 +103,24 @@ geo::ShapePtr parseXMLShape(const std::string& filename, std::string& error) if (min.size() == 3 && max.size() == 3) { - shape->addShape(geo::Box(geo::Vector3(min[0], min[1], min[2]), - geo::Vector3(max[0], max[1], max[2])), pose); + shape->addShape( + geo::Box(geo::Vector3(min[0], min[1], min[2]), geo::Vector3(max[0], max[1], max[2])), pose); } } else if (!size.empty()) { - geo::Vector3 v_size(size[0], size[1], size[2]); + geo::Vector3 const v_size(size[0], size[1], size[2]); shape->addShape(geo::Box(-v_size / 2, v_size / 2), pose); } else { - s_error << "In definition '" << filename << "': shape '" << shape_type << "' has no size property" << std::endl; + s_error << "In definition '" << filename << "': shape '" << shape_type << "' has no size property" + << '\n'; } - - } else { - s_error << "In definition '" << filename << "': Unknown shape type: '" << shape_type << "'" << std::endl; + } + else + { + s_error << "In definition '" << filename << "': Unknown shape type: '" << shape_type << "'" << '\n'; } shape_xml = shape_xml->NextSiblingElement(); @@ -121,7 +128,7 @@ geo::ShapePtr parseXMLShape(const std::string& filename, std::string& error) error = s_error.str(); if (!error.empty()) - return geo::ShapePtr(); + return {}; return shape; } diff --git a/src/plugin_container.cpp b/src/plugin_container.cpp index c2e05955..cd900839 100644 --- a/src/plugin_container.cpp +++ b/src/plugin_container.cpp @@ -1,22 +1,21 @@ #include "ed/plugin_container.h" -#include #include "ed/init_data.h" #include "ed/plugin.h" +#include +#include -#include +#include -#include +#include namespace ed { // -------------------------------------------------------------------------------- -PluginContainer::PluginContainer(const TFBufferConstPtr& tf_buffer) - : class_loader_(nullptr), request_stop_(false), is_running_(false), cycle_duration_(0.1), loop_frequency_(10), - loop_frequency_max_(11), loop_frequency_min_(9), step_finished_(true), t_last_update_(0), tf_buffer_(tf_buffer), - loop_usage_status_(nullptr) +PluginContainer::PluginContainer(const rclcpp::Node::SharedPtr& node, const TFBufferConstPtr& tf_buffer) : + tf_buffer_(tf_buffer), node_(node), loop_usage_status_(nullptr) { } @@ -30,8 +29,8 @@ PluginContainer::~PluginContainer() thread_->join(); plugin_.reset(); - if (class_loader_) - delete class_loader_; + + delete class_loader_; } // -------------------------------------------------------------------------------- @@ -39,8 +38,8 @@ PluginContainer::~PluginContainer() PluginPtr PluginContainer::loadPlugin(const std::string& plugin_name, const std::string& plugin_type, InitData& init) { // Load the library - if (class_loader_) - delete class_loader_; + + delete class_loader_; class_loader_ = new pluginlib::ClassLoader("ed", "ed::Plugin"); // Create plugin @@ -48,12 +47,13 @@ PluginPtr PluginContainer::loadPlugin(const std::string& plugin_name, const std: init.config.addError("Could not find plugin with the type '" + plugin_type + "'."); else { - plugin_ = class_loader_->createInstance(plugin_type); + plugin_ = PluginPtr(class_loader_->createUnmanagedInstance(plugin_type)); if (plugin_) { name_ = plugin_name; plugin_->name_ = plugin_name; plugin_->tf_buffer_ = tf_buffer_; + plugin_->node_ = node_; configure(init, false); @@ -92,11 +92,11 @@ void PluginContainer::configure(InitData& init, bool reconfigure) tue::Configuration scoped_config = init.config.limitScope(); InitData scoped_init(init.properties, scoped_config); - plugin_->configure(scoped_config); // This call will become obsolete (TODO) + plugin_->configure(scoped_config); // This call will become obsolete (TODO) plugin_->initialize(scoped_init); // Read optional frequency (inside parameters is obsolete) - double freq_temp; + double freq_temp = NAN; if (init.config.value("frequency", freq_temp, tue::config::OPTIONAL)) init.config.addError("Specify parameter 'frequency' outside 'parameters'."); @@ -108,7 +108,7 @@ void PluginContainer::configure(InitData& init, bool reconfigure) tue::Configuration scoped_config; InitData scoped_init(init.properties, scoped_config); - plugin_->configure(scoped_config); // This call will become obsolete (TODO) + plugin_->configure(scoped_config); // This call will become obsolete (TODO) plugin_->initialize(scoped_init); if (scoped_config.hasError()) @@ -131,11 +131,11 @@ void PluginContainer::run() is_running_ = true; request_stop_ = false; - double innerloop_frequency = 1000; // TODO: magic number! + double const innerloop_frequency = 1000; // TODO: magic number! - ros::Rate r(loop_frequency_); - ros::Rate ir(innerloop_frequency); - while(!request_stop_) + rclcpp::WallRate r(loop_frequency_); + rclcpp::WallRate ir(innerloop_frequency); + while (!request_stop_) { if (!step()) // If not stepped, sleep short @@ -155,7 +155,7 @@ bool PluginContainer::step() // If we still have an update_request, it means the request is not yet handled, // so we have to skip this cycle (and wait until the world model has handled it) { - boost::lock_guard lg(mutex_update_request_); + boost::lock_guard const lg(mutex_update_request_); if (update_request_) return false; } @@ -164,7 +164,7 @@ bool PluginContainer::step() // Check if there is a new world. If so replace the current one with the new one { - boost::lock_guard lg(mutex_world_); + boost::lock_guard const lg(mutex_world_); if (world_new_) { world_current_ = world_new_; @@ -177,13 +177,13 @@ bool PluginContainer::step() if (world_current_) { - PluginInput data(*world_current_, world_deltas); + PluginInput const data(*world_current_, world_deltas); - UpdateRequestPtr update_request(new UpdateRequest); + UpdateRequestPtr const update_request(new UpdateRequest); loop_usage_status_->start(); { - ed::ErrorContext errc("Plugin:", name().c_str()); + ed::ErrorContext const errc("Plugin:", name().c_str()); // Old plugin_->process(*world_current_, *update_request); @@ -209,6 +209,4 @@ void PluginContainer::requestStop() // -------------------------------------------------------------------------------- -} - - +} // namespace ed diff --git a/src/rendering.cpp b/src/rendering.cpp index 56f58841..01fa35f2 100644 --- a/src/rendering.cpp +++ b/src/rendering.cpp @@ -1,38 +1,39 @@ -// ROS -#include - // TU/e Robotics -#include -#include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include // ED #include "ed/entity.h" #include "ed/rendering.h" +#include "ed/types.h" #include "ed/world_model.h" -namespace ed { - - -float COLORS[27][3] = { { 0.6, 0.6, 0.6}, { 0.6, 0.6, 0.4}, { 0.6, 0.6, 0.2}, - { 0.6, 0.4, 0.6}, { 0.6, 0.4, 0.4}, { 0.6, 0.4, 0.2}, - { 0.6, 0.2, 0.6}, { 0.6, 0.2, 0.4}, { 0.6, 0.2, 0.2}, - { 0.4, 0.6, 0.6}, { 0.4, 0.6, 0.4}, { 0.4, 0.6, 0.2}, - { 0.4, 0.4, 0.6}, { 0.4, 0.4, 0.4}, { 0.4, 0.4, 0.2}, - { 0.4, 0.2, 0.6}, { 0.4, 0.2, 0.4}, { 0.4, 0.2, 0.2}, - { 0.2, 0.6, 0.6}, { 0.2, 0.6, 0.4}, { 0.2, 0.6, 0.2}, - { 0.2, 0.4, 0.6}, { 0.2, 0.4, 0.4}, { 0.2, 0.4, 0.2}, - { 0.2, 0.2, 0.6}, { 0.2, 0.2, 0.4}, { 0.2, 0.2, 0.2} }; +namespace ed +{ +static float COLORS[27][3] = {{0.6, 0.6, 0.6}, {0.6, 0.6, 0.4}, {0.6, 0.6, 0.2}, {0.6, 0.4, 0.6}, {0.6, 0.4, 0.4}, + {0.6, 0.4, 0.2}, {0.6, 0.2, 0.6}, {0.6, 0.2, 0.4}, {0.6, 0.2, 0.2}, {0.4, 0.6, 0.6}, + {0.4, 0.6, 0.4}, {0.4, 0.6, 0.2}, {0.4, 0.4, 0.6}, {0.4, 0.4, 0.4}, {0.4, 0.4, 0.2}, + {0.4, 0.2, 0.6}, {0.4, 0.2, 0.4}, {0.4, 0.2, 0.2}, {0.2, 0.6, 0.6}, {0.2, 0.6, 0.4}, + {0.2, 0.6, 0.2}, {0.2, 0.4, 0.6}, {0.2, 0.4, 0.4}, {0.2, 0.4, 0.2}, {0.2, 0.2, 0.6}, + {0.2, 0.2, 0.4}, {0.2, 0.2, 0.2}}; class SampleRenderResult : public geo::RenderResult { public: - - SampleRenderResult(cv::Mat& z_buffer, cv::Mat& image) - : geo::RenderResult(z_buffer.cols, z_buffer.rows), z_buffer_(z_buffer), image_(image) + SampleRenderResult(cv::Mat& z_buffer, cv::Mat& image) : + geo::RenderResult(z_buffer.cols, z_buffer.rows), z_buffer_(z_buffer), image_(image) { } @@ -43,18 +44,18 @@ class SampleRenderResult : public geo::RenderResult vals_.assign(mesh_->getTriangleIs().size(), -1); } - inline void setColor(const cv::Vec3b& color) { color_ = color; } + void setColor(const cv::Vec3b& color) { color_ = color; } - void renderPixel(int x, int y, float depth, int i_triangle) + void renderPixel(int x, int y, float depth, int i_triangle) override { - float old_depth = z_buffer_.at(y, x); + float const old_depth = z_buffer_.at(y, x); if (old_depth == 0. || depth < old_depth) { z_buffer_.at(y, x) = depth; if (vals_[i_triangle] < 0) { - geo::Vec3 n = mesh_->getTriangleNormal(i_triangle); + geo::Vec3 const n = mesh_->getTriangleNormal(i_triangle); // Small color difference between surfaces vals_[i_triangle] = (1 + n.dot(geo::Vec3(0, 0.3, -1).normalized())) / 2; @@ -65,21 +66,18 @@ class SampleRenderResult : public geo::RenderResult } protected: - cv::Mat& z_buffer_; cv::Mat& image_; - const geo::Mesh* mesh_; + const geo::Mesh* mesh_{}; cv::Vec3b color_; std::vector vals_; - }; - -unsigned int djb2(const std::string& str) +static unsigned int djb2(const std::string& str) { int hash = 5381; - for(unsigned int i = 0; i < str.size(); ++i) - hash = ((hash << 5) + hash) + str[i]; /* hash * 33 + c */ + for (char const i : str) + hash = ((hash << 5) + hash) + i; /* hash * 33 + c */ if (hash < 0) hash = -hash; @@ -96,7 +94,12 @@ unsigned int djb2(const std::string& str) * @param res Renderresult, which stores the renderer image * @param flatten Flatten all the meshes to the groundplane (default: false) */ -void renderMesh(const geo::DepthCamera& cam, const geo::Pose3D& pose, const geo::Mesh& mesh, const cv::Vec3b& color, SampleRenderResult& res, bool flatten = false) +static void renderMesh(const geo::DepthCamera& cam, + const geo::Pose3D& pose, + const geo::Mesh& mesh, + const cv::Vec3b& color, + SampleRenderResult& res, + bool flatten = false) { geo::RenderOptions opt; res.setColor(color); @@ -125,81 +128,87 @@ void renderMesh(const geo::DepthCamera& cam, const geo::Pose3D& pose, const geo: } // Might it be nicer to separate rendering of the colored image and the depth image? -bool renderWorldModel(const ed::WorldModel& world_model, const enum ShowVolumes show_volumes, - const geo::DepthCamera& cam, const geo::Pose3D& cam_pose_inv, - cv::Mat& depth_image, cv::Mat& image, bool flatten) +bool renderWorldModel(const ed::WorldModel& world_model, + const enum ShowVolumes show_volumes, + const geo::DepthCamera& cam, + const geo::Pose3D& cam_pose_inv, + cv::Mat& depth_image, + cv::Mat& image, + bool flatten) { if (depth_image.rows != image.rows || depth_image.cols != image.cols) { - throw std::invalid_argument("Depth image and image must be of the same size"); + throw std::invalid_argument("Depth image and image must be of the same size"); } SampleRenderResult res(depth_image, image); - geo::RenderOptions opt; + geo::RenderOptions const opt; // Draw axis constexpr double al = 0.25; // axis length (m) constexpr double at = 0.01; // axis thickness (m) - geo::Mesh x_box = geo::Box(geo::Vector3(0, -at, -at), geo::Vector3(al, at, at)).getMesh(); - geo::Mesh y_box = geo::Box(geo::Vector3(-at, 0, -at), geo::Vector3(at, al, at)).getMesh(); - geo::Mesh z_box = geo::Box(geo::Vector3(-at, -at, 0), geo::Vector3(at, at, al)).getMesh(); + geo::Mesh const x_box = geo::Box(geo::Vector3(0, -at, -at), geo::Vector3(al, at, at)).getMesh(); + geo::Mesh const y_box = geo::Box(geo::Vector3(-at, 0, -at), geo::Vector3(at, al, at)).getMesh(); + geo::Mesh const z_box = geo::Box(geo::Vector3(-at, -at, 0), geo::Vector3(at, at, al)).getMesh(); renderMesh(cam, cam_pose_inv, x_box, cv::Vec3b(0, 0, 255), res, flatten); renderMesh(cam, cam_pose_inv, y_box, cv::Vec3b(0, 255, 0), res, flatten); renderMesh(cam, cam_pose_inv, z_box, cv::Vec3b(255, 0, 0), res, flatten); - for(ed::WorldModel::const_iterator it = world_model.begin(); it != world_model.end(); ++it) + for (const auto& e : world_model) { - const ed::EntityConstPtr& e = *it; const std::string& id = e->id().str(); - if (e->visual() && e->has_pose() && !e->hasFlag("self") && (id.size() < 5 || id.substr(id.size() - 5) != "floor")) // Filter ground plane + if (e->visual() && e->has_pose() && !e->hasFlag("self") && + (id.size() < 5 || id.substr(id.size() - 5) != "floor")) // Filter ground plane { - if (show_volumes == RoomVolumes && (id.size() < 4 || id.substr(0, 4) != "wall")) continue; + if (show_volumes == RoomVolumes && (id.size() < 4 || id.substr(0, 4) != "wall")) + continue; cv::Vec3b color; tue::config::Reader config(e->data()); if (config.readGroup("color")) { - double r, g, b; + double r = NAN; + double g = NAN; + double b = NAN; if (config.value("red", r) && config.value("green", g) && config.value("blue", b)) color = cv::Vec3b(255 * b, 255 * g, 255 * r); config.endGroup(); } else { - int i_color = djb2(id) % 27; + int const i_color = djb2(id) % 27; color = cv::Vec3b(255 * COLORS[i_color][2], 255 * COLORS[i_color][1], 255 * COLORS[i_color][0]); } - geo::Pose3D pose = cam_pose_inv * e->pose(); + geo::Pose3D const pose = cam_pose_inv * e->pose(); renderMesh(cam, pose, e->visual()->getMesh(), color, res, flatten); // Render volumes if (show_volumes == ModelVolumes && !e->volumes().empty()) { - for (std::map::const_iterator it = e->volumes().begin(); it != e->volumes().end(); ++it) + for (const auto& it : e->volumes()) { - renderMesh(cam, pose, it->second->getMesh(), cv::Vec3b(0, 0, 255), res, flatten); // Red + renderMesh(cam, pose, it.second->getMesh(), cv::Vec3b(0, 0, 255), res, flatten); // Red } } } else if (show_volumes == RoomVolumes && e->types().find("room") != e->types().end()) { - geo::Pose3D pose = cam_pose_inv * e->pose(); - for (std::map::const_iterator it = e->volumes().begin(); it != e->volumes().end(); ++it) + geo::Pose3D const pose = cam_pose_inv * e->pose(); + for (const auto& it : e->volumes()) { - renderMesh(cam, pose, it->second->getMesh(), cv::Vec3b(0, 0, 255), res, flatten); // Red + renderMesh(cam, pose, it.second->getMesh(), cv::Vec3b(0, 0, 255), res, flatten); // Red } } - } return true; } -} +} // namespace ed diff --git a/src/serialization/serialization.cpp b/src/serialization/serialization.cpp index bfd5c8c2..ca7d16e1 100644 --- a/src/serialization/serialization.cpp +++ b/src/serialization/serialization.cpp @@ -1,49 +1,58 @@ #include "ed/serialization/serialization.h" #include "ed/mask.h" -#include "ed/world_model.h" -#include "ed/update_request.h" -#include "ed/entity.h" #include "ed/convex_hull_calc.h" +#include "ed/update_request.h" +#include +#include +#include +#include +#include +#include +#include +#include #include -#include +#include -#include #include +#include #include #include +#include +#include +#include namespace ed { // ---------------------------------------------------------------------------------------------------- -//void serialize(const WorldModel& wm, ed::io::Writer& w, unsigned long since_revision) +// void serialize(const WorldModel& wm, ed::io::Writer& w, unsigned long since_revision) //{ //} //// ---------------------------------------------------------------------------------------------------- -//void serialize(const Entity& wm, ed::io::Writer& w, unsigned long since_revision) +// void serialize(const Entity& wm, ed::io::Writer& w, unsigned long since_revision) //{ //} // ---------------------------------------------------------------------------------------------------- -bool deserialize(io::Reader &r, UpdateRequest& req) +bool deserialize(io::Reader& r, UpdateRequest& req) { if (r.readArray("entities")) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { std::string id; if (!r.readValue("id", id)) { - std::cout << "Deserialze: Entities should have field 'id'" << std::endl; + std::cout << "Deserialze: Entities should have field 'id'" << '\n'; return false; } @@ -53,7 +62,7 @@ bool deserialize(io::Reader &r, UpdateRequest& req) req.setType(id, type); } - double existence_prob; + double existence_prob = NAN; if (r.readValue("existence_prob", existence_prob)) { req.setExistenceProbability(id, existence_prob); @@ -87,7 +96,7 @@ bool deserialize(io::Reader &r, UpdateRequest& req) if (r.readGroup("mesh")) { - geo::ShapePtr shape(new geo::Shape); + geo::ShapePtr const shape(new geo::Shape); ed::deserialize(r, *shape); req.setVisual(id, shape); req.setCollision(id, shape); @@ -105,38 +114,39 @@ bool deserialize(io::Reader &r, UpdateRequest& req) req.addData(id, cfg.data()); } -// if (r.readArray("properties")) -// { -// while(r.nextArrayItem()) -// { -// std::string prop_name; -// if (!r.readValue("name", prop_name)) -// continue; - -// const ed::PropertyKeyDBEntry* entry = data.world.getPropertyInfo(prop_name); -// if (!entry) -// { -// error += "For entity '" + id + "': unknown property '" + prop_name +"'.\n"; -// continue; -// } - -// if (!entry->info->serializable()) -// { -// error += "For entity '" + id + "': property '" + prop_name +"' is not serializable.\n"; -// continue; -// } - -// ed::Variant value; -// if (entry->info->deserialize(r, value)) -// { -// req.setProperty(id, entry, value); -// ROS_INFO_STREAM("Sync plugin: setProperty " << id); -// } else -// error += "For entity '" + id + "': deserialization of property '" + prop_name +"' failed.\n"; -// } - -// r.endArray(); -// } + // if (r.readArray("properties")) + // { + // while(r.nextArrayItem()) + // { + // std::string prop_name; + // if (!r.readValue("name", prop_name)) + // continue; + + // const ed::PropertyKeyDBEntry* entry = data.world.getPropertyInfo(prop_name); + // if (!entry) + // { + // error += "For entity '" + id + "': unknown property '" + prop_name +"'.\n"; + // continue; + // } + + // if (!entry->info->serializable()) + // { + // error += "For entity '" + id + "': property '" + prop_name +"' is not + // serializable.\n"; continue; + // } + + // ed::Variant value; + // if (entry->info->deserialize(r, value)) + // { + // req.setProperty(id, entry, value); + // ROS_INFO_STREAM("Sync plugin: setProperty " << id); + // } else + // error += "For entity '" + id + "': deserialization of property '" + prop_name +"' + // failed.\n"; + // } + + // r.endArray(); + // } } r.endArray(); @@ -153,7 +163,7 @@ void serialize(const geo::Pose3D& pose, ed::io::Writer& w) w.writeValue("y", pose.t.y); w.writeValue("z", pose.t.z); - geo::Quaternion q = pose.getQuaternion(); + geo::Quaternion const q = pose.getQuaternion(); w.writeValue("qx", q.x); w.writeValue("qy", q.y); w.writeValue("qz", q.z); @@ -221,22 +231,22 @@ bool deserialize(tue::config::Reader& r, const std::string& group, geo::Pose3D& r.value("y", pose.t.y, tue::config::OPTIONAL); r.value("z", pose.t.z, tue::config::OPTIONAL); - double roll = 0, pitch = 0, yaw = 0; - r.value("X", roll, tue::config::OPTIONAL); + double roll = 0; + double pitch = 0; + double yaw = 0; + r.value("X", roll, tue::config::OPTIONAL); r.value("Y", pitch, tue::config::OPTIONAL); - r.value("Z", yaw, tue::config::OPTIONAL); - r.value("roll", roll, tue::config::OPTIONAL); + r.value("Z", yaw, tue::config::OPTIONAL); + r.value("roll", roll, tue::config::OPTIONAL); r.value("pitch", pitch, tue::config::OPTIONAL); - r.value("yaw", yaw, tue::config::OPTIONAL); + r.value("yaw", yaw, tue::config::OPTIONAL); // Set rotation pose.R.setRPY(roll, pitch, yaw); geo::Quaternion q; - if (r.value("qx", q.x, tue::config::OPTIONAL) - && r.value("qy", q.y, tue::config::OPTIONAL) - && r.value("qz", q.z, tue::config::OPTIONAL) - && r.value("qw", q.w, tue::config::OPTIONAL)) + if (r.value("qx", q.x, tue::config::OPTIONAL) && r.value("qy", q.y, tue::config::OPTIONAL) && + r.value("qz", q.z, tue::config::OPTIONAL) && r.value("qw", q.w, tue::config::OPTIONAL)) { pose.R.setRotation(q); } @@ -265,10 +275,11 @@ bool deserialize(tue::config::Reader& r, const std::string& group, geo::Vec3& p) void serialize(const ConvexHull& ch, ed::io::Writer& w) { w.writeArray("points"); - for (std::vector::const_iterator it = ch.points.begin(); it != ch.points.end(); ++it) + for (const auto& point : ch.points) { w.addArrayItem(); - w.writeValue("x", it->x); w.writeValue("y", it->y); + w.writeValue("x", point.x); + w.writeValue("y", point.y); w.endArrayItem(); } w.endArray(); @@ -282,7 +293,7 @@ bool deserialize(ed::io::Reader& r, ConvexHull& ch) { if (r.readArray("points")) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { geo::Vec2f p; r.readValue("x", p.x); @@ -307,24 +318,25 @@ void serialize(const geo::Shape& s, ed::io::Writer& w) { w.writeArray("vertices"); const std::vector& vertices = s.getMesh().getPoints(); - for(std::vector::const_iterator it = vertices.begin(); it != vertices.end(); ++it) + for (const auto& vertice : vertices) { w.addArrayItem(); - w.writeValue("x", it->x); w.writeValue("y", it->y); w.writeValue("z", it->z); + w.writeValue("x", vertice.x); + w.writeValue("y", vertice.y); + w.writeValue("z", vertice.z); w.endArrayItem(); } w.endArray(); w.writeArray("triangles"); const std::vector& triangles = s.getMesh().getTriangleIs(); - for(unsigned int i = 0; i < triangles.size(); ++i) + for (auto triangle : triangles) { w.addArrayItem(); - w.writeValue("i1", static_cast(triangles[i].i1_)); - w.writeValue("i2", static_cast(triangles[i].i2_)); - w.writeValue("i3", static_cast(triangles[i].i3_)); + w.writeValue("i1", static_cast(triangle.i1_)); + w.writeValue("i2", static_cast(triangle.i2_)); + w.writeValue("i3", static_cast(triangle.i3_)); w.endArrayItem(); - } w.endArray(); } @@ -338,7 +350,7 @@ bool deserialize(ed::io::Reader& r, geo::Shape& s) // Vertices if (r.readArray("vertices")) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { geo::Vector3 p; r.readValue("x", p.x); @@ -355,9 +367,11 @@ bool deserialize(ed::io::Reader& r, geo::Shape& s) // Triangles if (r.readArray("triangles")) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { - int i1, i2, i3; + int i1 = 0; + int i2 = 0; + int i3 = 0; r.readValue("i1", i1); r.readValue("i2", i2); r.readValue("i3", i3); @@ -383,11 +397,13 @@ bool deserialize(tue::config::Reader& r_orig, const std::string& group, geo::Sha if (r.readArray(group)) { - while(r.nextArrayItem()) + while (r.nextArrayItem()) { if (r.readGroup("box")) { - geo::Vec3 min, max, size; + geo::Vec3 min; + geo::Vec3 max; + geo::Vec3 size; if (deserialize(r, "min", min)) { if (!deserialize(r, "max", max)) @@ -396,7 +412,7 @@ bool deserialize(tue::config::Reader& r_orig, const std::string& group, geo::Sha else if (deserialize(r, "size", size)) { min = -0.5 * size; - max = 0.5 * size; + max = 0.5 * size; } else { @@ -413,11 +429,13 @@ bool deserialize(tue::config::Reader& r_orig, const std::string& group, geo::Sha } r.endArray(); } - else if(r.readGroup(group)) + else if (r.readGroup(group)) { if (r.readGroup("box")) { - geo::Vec3 min, max, size; + geo::Vec3 min; + geo::Vec3 max; + geo::Vec3 size; if (deserialize(r, "min", min)) { if (!deserialize(r, "max", max)) @@ -426,7 +444,7 @@ bool deserialize(tue::config::Reader& r_orig, const std::string& group, geo::Sha else if (deserialize(r, "size", size)) { min = -0.5 * size; - max = 0.5 * size; + max = 0.5 * size; } else { @@ -456,18 +474,19 @@ bool deserialize(tue::config::Reader& r_orig, const std::string& group, geo::Sha void serializeTimestamp(double time, ed::io::Writer& w) { - w.writeValue("sec", (int)time); - w.writeValue("nsec", (int)((time - (int)time) * 1e9)); + w.writeValue("sec", static_cast(time)); + w.writeValue("nsec", static_cast((time - static_cast(time)) * 1e9)); } // ---------------------------------------------------------------------------------------------------- bool deserializeTimestamp(ed::io::Reader& r, double& time) { - int sec, nsec; + int sec = 0; + int nsec = 0; r.readValue("sec", sec); r.readValue("nsec", nsec); - time = sec + (double)nsec / 1e9; + time = sec + (static_cast(nsec) / 1e9); return true; } @@ -484,15 +503,15 @@ void serialize(const ImageMask& mask, tue::serialization::OutputArchive& m) // Determine size int size = 0; - for(ImageMask::const_iterator it = mask.begin(); it != mask.end(); ++it) + for (ImageMask::const_iterator it = mask.begin(); it != mask.end(); ++it) ++size; m << size; - for(ImageMask::const_iterator it = mask.begin(); it != mask.end(); ++it) + for (ImageMask::const_iterator it = mask.begin(); it != mask.end(); ++it) { - cv::Point2i pt = it(); - m << pt.y * mask.width() + pt.x; + cv::Point2i const pt = it(); + m << (pt.y * mask.width()) + pt.x; } } @@ -500,20 +519,21 @@ void serialize(const ImageMask& mask, tue::serialization::OutputArchive& m) bool deserialize(tue::serialization::InputArchive& m, ImageMask& mask) { - int version; + int version = 0; m >> version; - int width, height; + int width = 0; + int height = 0; m >> width; m >> height; mask = ImageMask(width, height); - int size; + int size = 0; m >> size; - for(int i = 0; i < size; ++i) + for (int i = 0; i < size; ++i) { - int idx; + int idx = 0; m >> idx; mask.addPoint(idx % width, idx / width); @@ -522,8 +542,6 @@ bool deserialize(tue::serialization::InputArchive& m, ImageMask& mask) return true; } - - // ---------------------------------------------------------------------------------------------------- // // SERIALIZATION @@ -532,9 +550,9 @@ bool deserialize(tue::serialization::InputArchive& m, ImageMask& mask) // ---------------------------------------------------------------------------------------------------- -//void serialize(const WorldModel& wm, tue::config::Writer& w) +// void serialize(const WorldModel& wm, tue::config::Writer& w) //{ -// w.writeArray("entities"); +// w.writeArray("entities"); // for(WorldModel::const_iterator it = wm.begin(); it != wm.end(); ++it) // { @@ -574,19 +592,17 @@ bool deserialize(tue::serialization::InputArchive& m, ImageMask& mask) // // ---------------------------------------------------------------------------------------------------- - - // ---------------------------------------------------------------------------------------------------- -//void deserialize(tue::config::Reader& r, UpdateRequest& req) +// void deserialize(tue::config::Reader& r, UpdateRequest& req) //{ -// if (r.readArray("entities")) -// { -// while(r.nextArrayItem()) -// { -// std::string id; -// if (!r.value("id", id)) -// continue; +// if (r.readArray("entities")) +// { +// while(r.nextArrayItem()) +// { +// std::string id; +// if (!r.value("id", id)) +// continue; // if (r.readGroup("pose")) // { @@ -612,4 +628,4 @@ bool deserialize(tue::serialization::InputArchive& m, ImageMask& mask) // } //} -} +} // namespace ed diff --git a/src/server.cpp b/src/server.cpp index e7bb5565..5042e6d5 100644 --- a/src/server.cpp +++ b/src/server.cpp @@ -13,12 +13,10 @@ #include "ed/serialization/serialization.h" -#include #include +#include -#include - -#include +#include #include #include @@ -28,31 +26,32 @@ namespace ed // ---------------------------------------------------------------------------------------------------- -Server::Server() : world_model_(new WorldModel(&property_key_db_)) +Server::Server(const rclcpp::Node::SharedPtr& node) : + node_(node), world_model_(new WorldModel(&property_key_db_)), updater_(node) { updater_.setHardwareID("none"); - tf_buffer_ = ed::make_shared(); + tf_buffer_ = ed::make_shared(node_->get_clock()); tf_buffer_const_ = ed::const_pointer_cast(tf_buffer_); - tf_listener_ = ed::make_shared(*tf_buffer_); + tf_listener_ = ed::make_shared(*tf_buffer_, node_); } // ---------------------------------------------------------------------------------------------------- Server::~Server() { - ErrorContext errc("Server", "destructor"); + ErrorContext const errc("Server", "destructor"); } // ---------------------------------------------------------------------------------------------------- void Server::configure(tue::Configuration& config, bool /*reconfigure*/) { - ErrorContext errc("Server", "configure"); + ErrorContext const errc("Server", "configure"); if (config.readArray("plugins")) { - while(config.nextArrayItem()) + while (config.nextArrayItem()) { std::string name; if (!config.value("name", name)) @@ -63,7 +62,7 @@ void Server::configure(tue::Configuration& config, bool /*reconfigure*/) PluginContainerPtr plugin_container; - std::map::iterator it_plugin = plugin_containers_.find(name); + std::map::iterator const it_plugin = plugin_containers_.find(name); if (it_plugin == plugin_containers_.end()) { // Plugin does not yet exist @@ -91,12 +90,10 @@ void Server::configure(tue::Configuration& config, bool /*reconfigure*/) updater_.removeByName(name); continue; } - else - { - InitData init(property_key_db_, config); - plugin_container->configure(init, true); - updater_.add(plugin_container->getLoopUsageStatus()); - } + + InitData init(property_key_db_, config); + plugin_container->configure(init, true); + updater_.add(plugin_container->getLoopUsageStatus()); } if (config.hasError()) @@ -117,7 +114,7 @@ void Server::configure(tue::Configuration& config, bool /*reconfigure*/) { while (config.nextArrayItem()) { - ed::UpdateRequestPtr req(new UpdateRequest); + ed::UpdateRequestPtr const req(new UpdateRequest); std::stringstream error; if (!model_loader_.create(config.data(), "", "", *req, error)) { @@ -127,7 +124,7 @@ void Server::configure(tue::Configuration& config, bool /*reconfigure*/) // Create world model copy (shallow) boost::unique_lock ul(mutex_world_); - WorldModelPtr new_world_model = ed::make_shared(*world_model_); + WorldModelPtr const new_world_model = ed::make_shared(*world_model_); new_world_model->update(*req); @@ -141,10 +138,9 @@ void Server::configure(tue::Configuration& config, bool /*reconfigure*/) void Server::initialize() { - if (pub_stats_.getTopic().empty()) + if (!pub_stats_) { - ros::NodeHandle nh; - pub_stats_ = nh.advertise("ed/stats", 10); + pub_stats_ = node_->create_publisher("ed/stats", 10); } } @@ -152,25 +148,27 @@ void Server::initialize() void Server::reset(bool keep_all_shapes) { - ErrorContext errc("Server", "reset"); + ErrorContext const errc("Server", "reset"); - // Create init world request, such that we can check which entities we have to keep in the world model - UpdateRequestPtr req_init_world(new UpdateRequest); + // Create init world request, such that we can check which entities we have to + // keep in the world model + UpdateRequestPtr const req_init_world(new UpdateRequest); std::stringstream error; if (!model_loader_.create("_root", world_name_, *req_init_world, error, true)) { - ROS_ERROR_STREAM("[ED] Could not initialize world: " << error.str()); + RCLCPP_ERROR_STREAM(node_->get_logger(), "[ED] Could not initialize world: " << error.str()); } // Prepare deletion request - UpdateRequestPtr req_delete(new UpdateRequest); - WorldModelConstPtr wm = world_model(); - for(WorldModel::const_iterator it = wm->begin(); it != wm->end(); ++it) + UpdateRequestPtr const req_delete(new UpdateRequest); + WorldModelConstPtr const wm = world_model(); + for (WorldModel::const_iterator it = wm->begin(); it != wm->end(); ++it) { // Only remove entities that are NOT in the initial world model const ed::EntityConstPtr& e = *it; - if (e->id().str().substr(0, 6) == "sergio" || e->id().str().substr(0, 5) == "amigo" || e->id().str().substr(0, 4) == "hero") // TODO: robocup hack + if (e->id().str().substr(0, 6) == "sergio" || e->id().str().substr(0, 5) == "amigo" || + e->id().str().substr(0, 4) == "hero") // TODO: robocup hack continue; if (keep_all_shapes && e->visual()) @@ -182,7 +180,7 @@ void Server::reset(bool keep_all_shapes) // Create world model copy boost::unique_lock ul(mutex_world_); - WorldModelPtr new_world_model = ed::make_shared(*world_model_); + WorldModelPtr const new_world_model = ed::make_shared(*world_model_); // Apply the deletion request new_world_model->update(*req_init_world); @@ -193,7 +191,9 @@ void Server::reset(bool keep_all_shapes) ul.unlock(); // Notify plugins - for(std::map::iterator it = plugin_containers_.begin(); it != plugin_containers_.end(); ++it) + for (std::map::iterator it = plugin_containers_.begin(); + it != plugin_containers_.end(); + ++it) { const PluginContainerPtr& c = it->second; c->addDelta(req_init_world); @@ -206,7 +206,7 @@ void Server::reset(bool keep_all_shapes) PluginContainerPtr Server::loadPlugin(const std::string& plugin_name, tue::Configuration config) { - ErrorContext errc("Server loadPlugin", plugin_name.c_str()); + ErrorContext const errc("Server loadPlugin", plugin_name.c_str()); config.setErrorContext("While loading plugin '" + plugin_name + "': "); @@ -221,7 +221,7 @@ PluginContainerPtr Server::loadPlugin(const std::string& plugin_name, tue::Confi } // Create a plugin container - PluginContainerPtr container = ed::make_shared(tf_buffer_const_); + PluginContainerPtr container = ed::make_shared(node_, tf_buffer_const_); InitData init(property_key_db_, config); @@ -239,22 +239,24 @@ PluginContainerPtr Server::loadPlugin(const std::string& plugin_name, tue::Confi void Server::stepPlugins() { - ErrorContext errc("Server", "stepPlugins"); + ErrorContext const errc("Server", "stepPlugins"); WorldModelPtr new_world_model; // collect and apply all update requests std::vector plugins_with_requests; - for(std::map::iterator it = plugin_containers_.begin(); it != plugin_containers_.end(); ++it) + for (std::map::iterator it = plugin_containers_.begin(); + it != plugin_containers_.end(); + ++it) { - PluginContainerPtr c = it->second; + PluginContainerPtr const c = it->second; if (c->updateRequest()) { if (!new_world_model) { // Create world model copy (shallow) - boost::unique_lock ul(mutex_world_); + boost::unique_lock const ul(mutex_world_); new_world_model = ed::make_shared(*world_model_); } @@ -266,7 +268,9 @@ void Server::stepPlugins() if (new_world_model) { // Set the new (updated) world - for(std::map::iterator it = plugin_containers_.begin(); it != plugin_containers_.end(); ++it) + for (std::map::iterator it = plugin_containers_.begin(); + it != plugin_containers_.end(); + ++it) { const PluginContainerPtr& c = it->second; c->setWorld(new_world_model); @@ -276,9 +280,11 @@ void Server::stepPlugins() ul.unlock(); // Clear the requests of all plugins that had requests (which flags them to continue processing) - for(std::vector::iterator it = plugins_with_requests.begin(); it != plugins_with_requests.end(); ++it) + for (std::vector::iterator it = plugins_with_requests.begin(); + it != plugins_with_requests.end(); + ++it) { - PluginContainerPtr c = *it; + PluginContainerPtr const c = *it; c->clearUpdateRequest(); } } @@ -288,17 +294,19 @@ void Server::stepPlugins() void Server::update() { - ErrorContext errc("Server", "update"); + ErrorContext const errc("Server", "update"); // Create world model copy (shallow) boost::unique_lock ul(mutex_world_); - WorldModelPtr new_world_model = ed::make_shared(*world_model_); + WorldModelPtr const new_world_model = ed::make_shared(*world_model_); ul.unlock(); // Notify all plugins of the updated world model - for(std::map::iterator it = plugin_containers_.begin(); it != plugin_containers_.end(); ++it) + for (std::map::iterator it = plugin_containers_.begin(); + it != plugin_containers_.end(); + ++it) { - PluginContainerPtr c = it->second; + PluginContainerPtr const c = it->second; c->setWorld(new_world_model); } @@ -314,16 +322,18 @@ void Server::update(const ed::UpdateRequest& req) { // Create world model copy (shallow) boost::unique_lock ul(mutex_world_); - WorldModelPtr new_world_model = ed::make_shared(*world_model_); + WorldModelPtr const new_world_model = ed::make_shared(*world_model_); ul.unlock(); // Update the world model new_world_model->update(req); // Notify all plugins of the updated world model - for(std::map::iterator it = plugin_containers_.begin(); it != plugin_containers_.end(); ++it) + for (std::map::iterator it = plugin_containers_.begin(); + it != plugin_containers_.end(); + ++it) { - PluginContainerPtr c = it->second; + PluginContainerPtr const c = it->second; c->setWorld(new_world_model); } @@ -353,7 +363,7 @@ void Server::update(const std::string& update_str, std::string& error) if (cfg.readArray("entities")) { - while(cfg.nextArrayItem()) + while (cfg.nextArrayItem()) { std::string id; if (!cfg.value("id", id)) @@ -366,7 +376,9 @@ void Server::update(const std::string& update_str, std::string& error) if (!cfg.value("x", pose.t.x) || !cfg.value("y", pose.t.y) || !cfg.value("z", pose.t.z)) continue; - double rx = 0, ry = 0, rz = 0; + double rx = 0; + double ry = 0; + double rz = 0; cfg.value("rx", rx, tue::config::OPTIONAL); cfg.value("ry", ry, tue::config::OPTIONAL); cfg.value("rz", rz, tue::config::OPTIONAL); @@ -393,16 +405,18 @@ void Server::update(const std::string& update_str, std::string& error) // Create world model copy (shallow) boost::unique_lock ul(mutex_world_); - WorldModelPtr new_world_model = ed::make_shared(*world_model_); + WorldModelPtr const new_world_model = ed::make_shared(*world_model_); ul.unlock(); // Update the world model new_world_model->update(req); // Notify all plugins of the updated world model - for(std::map::iterator it = plugin_containers_.begin(); it != plugin_containers_.end(); ++it) + for (std::map::iterator it = plugin_containers_.begin(); + it != plugin_containers_.end(); + ++it) { - PluginContainerPtr c = it->second; + PluginContainerPtr const c = it->second; c->setWorld(new_world_model); } @@ -415,17 +429,17 @@ void Server::update(const std::string& update_str, std::string& error) void Server::initializeWorld() { - ed::UpdateRequestPtr req(new UpdateRequest); + ed::UpdateRequestPtr const req(new UpdateRequest); std::stringstream error; if (!model_loader_.create("_root", world_name_, *req, error, true)) { - ROS_ERROR_STREAM("[ED] Could not initialize world: " << error.str()); + RCLCPP_ERROR_STREAM(node_->get_logger(), "[ED] Could not initialize world: " << error.str()); return; } // Create world model copy (shallow) - boost::unique_lock ul(mutex_world_); - WorldModelPtr new_world_model = boost::make_shared(*world_model_); + boost::unique_lock const ul(mutex_world_); + WorldModelPtr const new_world_model = boost::make_shared(*world_model_); new_world_model->update(*req); @@ -436,15 +450,15 @@ void Server::initializeWorld() void Server::storeEntityMeasurements(const std::string& path) const { - WorldModelConstPtr wm = world_model(); - for(WorldModel::const_iterator it = wm->begin(); it != wm->end(); ++it) + WorldModelConstPtr const wm = world_model(); + for (WorldModel::const_iterator it = wm->begin(); it != wm->end(); ++it) { const EntityConstPtr& e = *it; - MeasurementConstPtr msr = e->lastMeasurement(); + MeasurementConstPtr const msr = e->lastMeasurement(); if (!msr) continue; - std::string filename = path + "/" + e->id().str(); + std::string const filename = path + "/" + e->id().str(); if (!write(filename, *msr)) { std::cout << "Saving measurement failed." << std::endl; @@ -456,26 +470,28 @@ void Server::storeEntityMeasurements(const std::string& path) const void Server::publishStatistics() { - ErrorContext errc("Server", "publishStatistics"); + ErrorContext const errc("Server", "publishStatistics"); std::stringstream s; s << "[plugins]" << std::endl; - for(std::map::const_iterator it = plugin_containers_.begin(); it != plugin_containers_.end(); ++it) + for (std::map::const_iterator it = plugin_containers_.begin(); + it != plugin_containers_.end(); + ++it) { const PluginContainerPtr& p = it->second; // Calculate CPU usage percentage - double cpu_perc = p->getLoopUsageStatus().getTimer().getLoopUsagePercentage() * 100; + double const cpu_perc = p->getLoopUsageStatus().getTimer().getLoopUsagePercentage() * 100; - s << " " << p->name() << ": " << std::fixed << cpu_perc << " % (" << std::defaultfloat << p->loopFrequency() << " hz)" << std::endl; + s << " " << p->name() << ": " << std::fixed << cpu_perc << " % (" << std::defaultfloat << p->loopFrequency() + << " hz)" << std::endl; } - - std_msgs::String msg; + std_msgs::msg::String msg; msg.data = s.str(); - pub_stats_.publish(msg); + pub_stats_->publish(msg); updater_.force_update(); } -} +} // namespace ed diff --git a/src/transform_cache.cpp b/src/transform_cache.cpp index e4b06b69..1aba060a 100644 --- a/src/transform_cache.cpp +++ b/src/transform_cache.cpp @@ -1,18 +1,24 @@ +#include + #include "ed/relations/transform_cache.h" +#include "ed/time.h" +#include "ed/time_cache.h" +#include namespace ed { // -------------------------------------------------------------------------------------------------------------- -inline float clamp(float x, float a, float b) { +static inline float clamp(float x, float a, float b) +{ return x < a ? a : (x > b ? b : x); } // -------------------------------------------------------------------------------------------------------------- // Taken from: http://www.arcsynthesis.org/gltut/Positioning/Tut08%20Interpolation.html -geo::Quaternion slerp(const geo::Quaternion& v0, const geo::Quaternion& v1, float alpha) +static geo::Quaternion slerp(const geo::Quaternion& v0, const geo::Quaternion& v1, float alpha) { float dot = v0.dot(v1); @@ -20,28 +26,28 @@ geo::Quaternion slerp(const geo::Quaternion& v0, const geo::Quaternion& v1, floa if (dot > DOT_THRESHOLD) { geo::Quaternion q; - q.x = (1 - alpha) * v0.getX() + alpha * v1.getX(); - q.y = (1 - alpha) * v0.getY() + alpha * v1.getY(); - q.z = (1 - alpha) * v0.getZ() + alpha * v1.getZ(); - q.w = (1 - alpha) * v0.getW() + alpha * v1.getW(); + q.x = ((1 - alpha) * v0.getX()) + (alpha * v1.getX()); + q.y = ((1 - alpha) * v0.getY()) + (alpha * v1.getY()); + q.z = ((1 - alpha) * v0.getZ()) + (alpha * v1.getZ()); + q.w = ((1 - alpha) * v0.getW()) + (alpha * v1.getW()); return q; } dot = clamp(dot, -1.0f, 1.0f); - float theta_0 = acosf(dot); - float theta = theta_0*alpha; + float const theta_0 = acosf(dot); + float const theta = theta_0 * alpha; - geo::Quaternion v2 = v1 - v0*dot; + geo::Quaternion v2 = v1 - v0 * dot; v2.normalize(); - return v0*cos(theta) + v2*sin(theta); + return v0 * std::cos(theta) + v2 * std::sin(theta); } // -------------------------------------------------------------------------------------------------------------- -void interpolate(const geo::Transform& t1, const geo::Transform& t2, float alpha, geo::Pose3D& result) +static void interpolate(const geo::Transform& t1, const geo::Transform& t2, float alpha, geo::Pose3D& result) { result.t = (1.0f - alpha) * t1.getOrigin() + alpha * t2.getOrigin(); result.R.setRotation(slerp(t1.getQuaternion(), t2.getQuaternion(), alpha)); @@ -49,22 +55,19 @@ void interpolate(const geo::Transform& t1, const geo::Transform& t2, float alpha // ---------------------------------------------------------------------------------------------------- -TransformCache::TransformCache() -{ -} +TransformCache::TransformCache() = default; // ---------------------------------------------------------------------------------------------------- -TransformCache::~TransformCache() -{ -} +TransformCache::~TransformCache() = default; // ---------------------------------------------------------------------------------------------------- bool TransformCache::calculateTransform(const Time& t, geo::Pose3D& tf) const { // Get lower and upper bound - TimeCache::const_iterator lower, upper; + TimeCache::const_iterator lower; + TimeCache::const_iterator upper; cache_.getLowerUpper(t, lower, upper); if (lower == cache_.end()) @@ -89,10 +92,10 @@ bool TransformCache::calculateTransform(const Time& t, geo::Pose3D& tf) const const geo::Pose3D& tf1 = lower->second; const geo::Pose3D& tf2 = upper->second; - double dt1 = t.seconds() - lower->first.seconds(); - double t_diff = upper->first.seconds() - lower->first.seconds(); + double const dt1 = t.seconds() - lower->first.seconds(); + double const t_diff = upper->first.seconds() - lower->first.seconds(); - float alpha = dt1 / t_diff; + float const alpha = dt1 / t_diff; interpolate(tf1, tf2, alpha, tf); } @@ -102,4 +105,3 @@ bool TransformCache::calculateTransform(const Time& t, geo::Pose3D& tf) const } } // end namespace ed - diff --git a/src/world_model.cpp b/src/world_model.cpp index 450fd948..6c583bf1 100644 --- a/src/world_model.cpp +++ b/src/world_model.cpp @@ -1,22 +1,35 @@ #include "ed/world_model.h" -#include "ed/update_request.h" #include "ed/entity.h" +#include "ed/measurement_convex_hull.h" +#include "ed/property.h" #include "ed/relation.h" +#include "ed/time.h" +#include "ed/types.h" +#include "ed/update_request.h" -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "ed/property_key_db.h" +#include "ed/uuid.h" namespace ed { // -------------------------------------------------------------------------------- -WorldModel::WorldModel(const PropertyKeyDB* prop_key_db) : revision_(0), property_info_db_(prop_key_db) -{ -} +WorldModel::WorldModel(const PropertyKeyDB* prop_key_db) : property_info_db_(prop_key_db) {} // -------------------------------------------------------------------------------- @@ -31,36 +44,36 @@ void WorldModel::update(const UpdateRequest& req) std::map new_entities; // Update associated measurements - for(std::map >::const_iterator it = req.measurements.begin(); it != req.measurements.end(); ++it) + for (const auto& measurement : req.measurements) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - const std::vector& measurements = it->second; - for(std::vector::const_iterator it2 = measurements.begin(); it2 != measurements.end(); ++it2) + EntityPtr const e = getOrAddEntity(measurement.first, new_entities); + const std::vector& measurements = measurement.second; + for (const auto& it2 : measurements) { - e->addMeasurement(*it2); + e->addMeasurement(it2); } } // Update poses - for(std::map::const_iterator it = req.poses.begin(); it != req.poses.end(); ++it) + for (const auto& pose : req.poses) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - e->setPose(it->second); + EntityPtr const e = getOrAddEntity(pose.first, new_entities); + e->setPose(pose.second); } for (const UUID& id : req.poses_removed) { - EntityPtr e = getOrAddEntity(id, new_entities); + EntityPtr const e = getOrAddEntity(id, new_entities); e->removePose(); } // Update visuals - for(std::map::const_iterator it = req.visuals.begin(); it != req.visuals.end(); ++it) + for (const auto& visual : req.visuals) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - e->setVisual(it->second); + EntityPtr const e = getOrAddEntity(visual.first, new_entities); + e->setVisual(visual.second); - Idx idx; + Idx idx = 0; if (findEntityIdx(e->id(), idx)) { entity_visual_revisions_[idx] = revision_; @@ -68,12 +81,12 @@ void WorldModel::update(const UpdateRequest& req) } // Update collisions - for(std::map::const_iterator it = req.collisions.begin(); it != req.collisions.end(); ++it) + for (const auto& collision : req.collisions) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - e->setCollision(it->second); + EntityPtr const e = getOrAddEntity(collision.first, new_entities); + e->setCollision(collision.second); - Idx idx; + Idx idx = 0; if (findEntityIdx(e->id(), idx)) { entity_collision_revisions_[idx] = revision_; @@ -81,16 +94,16 @@ void WorldModel::update(const UpdateRequest& req) } // Update convex hulls new - for(std::map >::const_iterator it = req.convex_hulls_new.begin(); it != req.convex_hulls_new.end(); ++it) + for (const auto& it : req.convex_hulls_new) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - for(std::map::const_iterator it2 = it->second.begin(); it2 != it->second.end(); ++it2) + EntityPtr const e = getOrAddEntity(it.first, new_entities); + for (const auto& it2 : it.second) { - const ed::MeasurementConvexHull& m = it2->second; - e->setConvexHull(m.convex_hull, m.pose, m.timestamp, it2->first); + const ed::MeasurementConvexHull& m = it2.second; + e->setConvexHull(m.convex_hull, m.pose, m.timestamp, it2.first); } - Idx idx; + Idx idx = 0; if (findEntityIdx(e->id(), idx)) { entity_visual_revisions_[idx] = revision_; @@ -99,27 +112,27 @@ void WorldModel::update(const UpdateRequest& req) } // Update volumes - for (std::map >::const_iterator it = req.volumes_removed.begin(); it != req.volumes_removed.end(); ++it ) + for (const auto& it : req.volumes_removed) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - const std::set& volume_names = it->second; - for (std::set::const_iterator it2 = volume_names.begin(); it2 != volume_names.end(); ++it2) - e->removeVolume(*it2); - Idx idx; + EntityPtr const e = getOrAddEntity(it.first, new_entities); + const std::set& volume_names = it.second; + for (const auto& volume_name : volume_names) + e->removeVolume(volume_name); + Idx idx = 0; if (findEntityIdx(e->id(), idx)) { entity_volumes_revisions_[idx] = revision_; } } - for (std::map >::const_iterator it = req.volumes_added.begin(); it != req.volumes_added.end(); ++it) + for (const auto& it : req.volumes_added) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - const std::map& volumes = it->second; - for (std::map::const_iterator it2 = volumes.begin(); it2 != volumes.end(); ++it2) + EntityPtr const e = getOrAddEntity(it.first, new_entities); + const std::map& volumes = it.second; + for (const auto& volume : volumes) { - e->addVolume(it2->first, it2->second); + e->addVolume(volume.first, volume.second); } - Idx idx; + Idx idx = 0; if (findEntityIdx(e->id(), idx)) { entity_volumes_revisions_[idx] = revision_; @@ -127,103 +140,103 @@ void WorldModel::update(const UpdateRequest& req) } // Update types - for(std::map::const_iterator it = req.types.begin(); it != req.types.end(); ++it) + for (const auto& type : req.types) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - e->setType(it->second); + EntityPtr const e = getOrAddEntity(type.first, new_entities); + e->setType(type.second); } - for(std::map >::const_iterator it = req.type_sets_added.begin(); it != req.type_sets_added.end(); ++it) + for (const auto& it : req.type_sets_added) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - const std::set& type_set = it->second; - for(std::set::const_iterator it2 = type_set.begin(); it2 != type_set.end(); ++it2) - e->addType(*it2); + EntityPtr const e = getOrAddEntity(it.first, new_entities); + const std::set& type_set = it.second; + for (const auto& it2 : type_set) + e->addType(it2); } - for(std::map >::const_iterator it = req.type_sets_removed.begin(); it != req.type_sets_removed.end(); ++it) + for (const auto& it : req.type_sets_removed) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - const std::set& type_set = it->second; - for(std::set::const_iterator it2 = type_set.begin(); it2 != type_set.end(); ++it2) - e->removeType(*it2); + EntityPtr const e = getOrAddEntity(it.first, new_entities); + const std::set& type_set = it.second; + for (const auto& it2 : type_set) + e->removeType(it2); } // Update existence probabilities - for(std::map::const_iterator it = req.existence_probabilities.begin(); it != req.existence_probabilities.end(); ++it) + for (const auto& existence_probabilitie : req.existence_probabilities) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - e->setExistenceProbability(it->second); + EntityPtr const e = getOrAddEntity(existence_probabilitie.first, new_entities); + e->setExistenceProbability(existence_probabilitie.second); } // Update last update timestamps - for(std::map::const_iterator it = req.last_update_timestamps.begin(); it != req.last_update_timestamps.end(); ++it) + for (const auto& last_update_timestamp : req.last_update_timestamps) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - e->setLastUpdateTimestamp(it->second); + EntityPtr const e = getOrAddEntity(last_update_timestamp.first, new_entities); + e->setLastUpdateTimestamp(last_update_timestamp.second); } // Update relations - for(std::map >::const_iterator it = req.relations.begin(); it != req.relations.end(); ++it) + for (const auto& relation : req.relations) { - Idx idx1; - if (findEntityIdx(it->first, idx1)) + Idx idx1 = 0; + if (findEntityIdx(relation.first, idx1)) { - const std::map& rels = it->second; - for(std::map::const_iterator it2 = rels.begin(); it2 != rels.end(); ++it2) + const std::map& rels = relation.second; + for (const auto& rel : rels) { - Idx idx2; - if (findEntityIdx(it2->first, idx2)) - setRelation(idx1, idx2, it2->second); + Idx idx2 = 0; + if (findEntityIdx(rel.first, idx2)) + setRelation(idx1, idx2, rel.second); else - std::cout << "WorldModel::update (relation): unknown entity: '" << it2->first << "'." << std::endl; + std::cout << "WorldModel::update (relation): unknown entity: '" << rel.first << "'." << '\n'; } } else - std::cout << "WorldModel::update (relation): unknown entity: '" << it->first << "'." << std::endl; + std::cout << "WorldModel::update (relation): unknown entity: '" << relation.first << "'." << '\n'; } // Update flags - for(std::map::const_iterator it = req.added_flags.begin(); it != req.added_flags.end(); ++it) + for (const auto& added_flag : req.added_flags) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - e->setFlag(it->second); + EntityPtr const e = getOrAddEntity(added_flag.first, new_entities); + e->setFlag(added_flag.second); } - for(std::map::const_iterator it = req.removed_flags.begin(); it != req.removed_flags.end(); ++it) + for (const auto& removed_flag : req.removed_flags) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - e->removeFlag(it->second); + EntityPtr const e = getOrAddEntity(removed_flag.first, new_entities); + e->removeFlag(removed_flag.second); } // Update additional info (data) - for(std::map::const_iterator it = req.datas.begin(); it != req.datas.end(); ++it) + for (const auto& data : req.datas) { - EntityPtr e = getOrAddEntity(it->first, new_entities); + EntityPtr const e = getOrAddEntity(data.first, new_entities); tue::config::DataPointer params; params.add(e->data()); - params.add(it->second); + params.add(data.second); e->setData(params); } - for(std::map >::const_iterator it = req.properties.begin(); it != req.properties.end(); ++it) + for (const auto& propertie : req.properties) { - EntityPtr e = getOrAddEntity(it->first, new_entities); - const std::map& props = it->second; + EntityPtr const e = getOrAddEntity(propertie.first, new_entities); + const std::map& props = propertie.second; - for(std::map::const_iterator it2 = props.begin(); it2 != props.end(); ++it2) + for (const auto& prop : props) { - const Property& p = it2->second; - e->setProperty(it2->first, p); + const Property& p = prop.second; + e->setProperty(prop.first, p); } } // Remove entities - for(std::set::const_iterator it = req.removed_entities.begin(); it != req.removed_entities.end(); ++it) + for (const auto& removed_entitie : req.removed_entities) { - removeEntity(*it); + removeEntity(removed_entitie); } } @@ -231,21 +244,21 @@ void WorldModel::update(const UpdateRequest& req) struct SearchNode { - SearchNode() {} + SearchNode() = default; - SearchNode(Idx parent_, Idx relation_, bool inverse_) - : parent(parent_), relation(relation_), inverse(inverse_) {} + SearchNode(Idx parent_, Idx relation_, bool inverse_) : parent(parent_), relation(relation_), inverse(inverse_) {} - Idx parent; - Idx relation; - bool inverse; + Idx parent{}; + Idx relation{}; + bool inverse{}; }; // -------------------------------------------------------------------------------- bool WorldModel::calculateTransform(const UUID& source, const UUID& target, const Time& time, geo::Pose3D& tf) const { - Idx s, t; + Idx s = 0; + Idx t = 0; if (!findEntityIdx(source, s) || !findEntityIdx(target, t)) return false; @@ -255,9 +268,9 @@ bool WorldModel::calculateTransform(const UUID& source, const UUID& target, cons Q.push(s); visited[s] = SearchNode(INVALID_IDX, INVALID_IDX, true); - while(!Q.empty()) + while (!Q.empty()) { - Idx n = Q.front(); + Idx const n = Q.front(); Q.pop(); if (n == t) @@ -269,7 +282,7 @@ bool WorldModel::calculateTransform(const UUID& source, const UUID& target, cons while (u != s) { - std::map::const_iterator it = visited.find(u); + auto const it = visited.find(u); const SearchNode& sn = it->second; const RelationConstPtr& r = relations_[sn.relation]; @@ -277,7 +290,9 @@ bool WorldModel::calculateTransform(const UUID& source, const UUID& target, cons geo::Pose3D tr; if (!r->calculateTransform(time, tr)) { - std::cout << "WorldModel::calculateTransform: transform could not be calculated. THIS SHOULD NEVER HAPPEN!" << std::endl; + std::cout << "WorldModel::calculateTransform: transform could not be calculated. THIS SHOULD NEVER " + "HAPPEN!" + << '\n'; return false; } @@ -294,24 +309,24 @@ bool WorldModel::calculateTransform(const UUID& source, const UUID& target, cons // Push all nodes that point to this node const std::map& transforms_to = entities_[n]->relationsTo(); - for(std::map::const_iterator it = transforms_to.begin(); it != transforms_to.end(); ++it) + for (auto it : transforms_to) { - Idx n2 = it->first; + Idx const n2 = it.first; if (visited.find(n2) == visited.end()) { - visited[n2] = SearchNode(n, it->second, false); + visited[n2] = SearchNode(n, it.second, false); Q.push(n2); } } // Push all nodes this node points to const std::map& transforms_from = entities_[n]->relationsFrom(); - for(std::map::const_iterator it = transforms_from.begin(); it != transforms_from.end(); ++it) + for (auto it : transforms_from) { - Idx n2 = it->first; + Idx const n2 = it.first; if (visited.find(n2) == visited.end()) { - visited[n2] = SearchNode(n, it->second, true); + visited[n2] = SearchNode(n, it.second, true); Q.push(n2); } } @@ -329,7 +344,7 @@ void WorldModel::setRelation(Idx parent, Idx child, const RelationConstPtr& r) if (!p || !c) { - std::cout << "[ED] ERROR: Invalid relation addition: parent or child does not exit." << std::endl; + std::cout << "[ED] ERROR: Invalid relation addition: parent or child does not exit." << '\n'; return; } @@ -338,8 +353,8 @@ void WorldModel::setRelation(Idx parent, Idx child, const RelationConstPtr& r) { r_idx = addRelation(r); - EntityPtr p_new(new Entity(*entities_[parent])); - EntityPtr c_new(new Entity(*entities_[child])); + EntityPtr const p_new(new Entity(*entities_[parent])); + EntityPtr const c_new(new Entity(*entities_[child])); p_new->setRelationTo(child, r_idx); c_new->setRelationFrom(parent, r_idx); @@ -353,7 +368,7 @@ void WorldModel::setRelation(Idx parent, Idx child, const RelationConstPtr& r) } // Update entity revisions - for(std::size_t i = entity_revisions_.size(); i < std::max(parent, child) + 1; ++i) + for (std::size_t i = entity_revisions_.size(); i < std::max(parent, child) + 1; ++i) entity_revisions_.push_back(0); entity_revisions_[parent] = revision_; entity_revisions_[child] = revision_; @@ -363,7 +378,7 @@ void WorldModel::setRelation(Idx parent, Idx child, const RelationConstPtr& r) Idx WorldModel::addRelation(const RelationConstPtr& r) { - Idx r_idx = relations_.size(); + Idx const r_idx = relations_.size(); relations_.push_back(r); return r_idx; } @@ -372,7 +387,7 @@ Idx WorldModel::addRelation(const RelationConstPtr& r) void WorldModel::setEntity(const UUID& id, const EntityConstPtr& e) { - std::map::const_iterator it_idx = entity_map_.find(id); + auto const it_idx = entity_map_.find(id); if (it_idx == entity_map_.end()) { addNewEntity(e); @@ -387,7 +402,7 @@ void WorldModel::setEntity(const UUID& id, const EntityConstPtr& e) void WorldModel::removeEntity(const UUID& id) { - std::map::iterator it_idx = entity_map_.find(id); + auto const it_idx = entity_map_.find(id); if (it_idx != entity_map_.end()) { entities_[it_idx->second].reset(); @@ -405,13 +420,13 @@ void WorldModel::removeEntity(const UUID& id) EntityPtr WorldModel::getOrAddEntity(const UUID& id, std::map& new_entities) { // Check if the id is already in the new_entities map. If so, return it - std::map::const_iterator it_e = new_entities.find(id); + auto const it_e = new_entities.find(id); if (it_e != new_entities.end()) return it_e->second; EntityPtr e; - Idx idx; + Idx idx = 0; if (findEntityIdx(id, idx)) { // Create a copy of the existing entity @@ -432,7 +447,7 @@ EntityPtr WorldModel::getOrAddEntity(const UUID& id, std::map& new_entities[id] = e; - for(std::size_t i = entity_revisions_.size(); i < idx + 1; ++i) + for (std::size_t i = entity_revisions_.size(); i < idx + 1; ++i) entity_revisions_.push_back(0); entity_revisions_[idx] = revision_; @@ -449,7 +464,7 @@ bool WorldModel::findEntityIdx(const UUID& id, Idx& idx) const return true; } - std::map::const_iterator it = entity_map_.find(id); + auto const it = entity_map_.find(id); if (it == entity_map_.end()) return false; @@ -462,7 +477,7 @@ bool WorldModel::findEntityIdx(const UUID& id, Idx& idx) const Idx WorldModel::addNewEntity(const EntityConstPtr& e) { - Idx idx; + Idx idx = 0; if (entity_empty_spots_.empty()) { idx = entities_.size(); @@ -493,6 +508,4 @@ const PropertyKeyDBEntry* WorldModel::getPropertyInfo(const std::string& name) c return property_info_db_->getPropertyKeyDBEntry(name); } -} - - +} // namespace ed diff --git a/src/world_model/transform_crawler.cpp b/src/world_model/transform_crawler.cpp index c8aad83d..e91379fc 100644 --- a/src/world_model/transform_crawler.cpp +++ b/src/world_model/transform_crawler.cpp @@ -1,20 +1,22 @@ #include "ed/world_model/transform_crawler.h" -#include "ed/world_model.h" #include "ed/entity.h" #include "ed/relation.h" +#include "ed/time.h" +#include "ed/types.h" +#include "ed/uuid.h" +#include "ed/world_model.h" +#include +#include -namespace ed -{ -namespace world_model +namespace ed::world_model { // ---------------------------------------------------------------------------------------------------- -TransformCrawler::TransformCrawler(const WorldModel& wm, const UUID& root_id, const Time& time) - : wm_(wm), time_(time) +TransformCrawler::TransformCrawler(const WorldModel& wm, const UUID& root_id, const Time& time) : wm_(wm), time_(time) { - Idx root_idx; + Idx root_idx = 0; if (wm_.findEntityIdx(root_id, root_idx)) { const EntityConstPtr& e = wm_.entities()[root_idx]; @@ -54,15 +56,15 @@ void TransformCrawler::pushChildren(const Entity& e, const geo::Pose3D& transfor { // Push all nodes that point to this node const std::map& transforms_to = e.relationsTo(); - for(std::map::const_iterator it = transforms_to.begin(); it != transforms_to.end(); ++it) + for (auto it : transforms_to) { - Idx n2 = it->first; + Idx const n2 = it.first; if (visited_.find(n2) == visited_.end()) { geo::Pose3D rel_transform; - RelationConstPtr r = wm_.relations()[it->second]; + RelationConstPtr const r = wm_.relations()[it.second]; if (r && r->calculateTransform(time_, rel_transform)) - queue_.push(Node(n2, transform * rel_transform)); + queue_.emplace(n2, transform * rel_transform); visited_.insert(n2); } @@ -70,15 +72,15 @@ void TransformCrawler::pushChildren(const Entity& e, const geo::Pose3D& transfor // Push all nodes this node points to const std::map& transforms_from = e.relationsFrom(); - for(std::map::const_iterator it = transforms_from.begin(); it != transforms_from.end(); ++it) + for (auto it : transforms_from) { - Idx n2 = it->first; + Idx const n2 = it.first; if (visited_.find(n2) == visited_.end()) { geo::Pose3D rel_transform; - RelationConstPtr r = wm_.relations()[it->second]; + RelationConstPtr const r = wm_.relations()[it.second]; if (r && r->calculateTransform(time_, rel_transform)) - queue_.push(Node(n2, transform * rel_transform.inverse())); + queue_.emplace(n2, transform * rel_transform.inverse()); visited_.insert(n2); } @@ -87,7 +89,4 @@ void TransformCrawler::pushChildren(const Entity& e, const geo::Pose3D& transfor // ---------------------------------------------------------------------------------------------------- -} // end namespace ed - -} // end namespace world_model - +} // namespace ed::world_model diff --git a/test/show_gui.cpp b/test/show_gui.cpp index bcd0092f..0ac8945b 100644 --- a/test/show_gui.cpp +++ b/test/show_gui.cpp @@ -1,15 +1,16 @@ -#include +#include #include -#include -#include +#include +#include -ros::ServiceClient client; +rclcpp::Node::SharedPtr g_node; +rclcpp::Client::SharedPtr client; std::string click_type; -void imageCallback(const tue_serialization::Binary::ConstPtr& msg) +void imageCallback(const tue_serialization_interfaces::msg::Binary::ConstSharedPtr msg) { - cv::Mat image = cv::imdecode(msg->data, cv::IMREAD_UNCHANGED); + cv::Mat const image = cv::imdecode(msg->data, cv::IMREAD_UNCHANGED); cv::imshow("map", image); cv::waitKey(3); } @@ -18,40 +19,30 @@ void mouseCallback(int event, int x, int y, int /*flags*/, void* /*ptr*/) { if (event == cv::EVENT_LBUTTONDOWN) { - ed_msgs::RaiseEvent srv_ev; - srv_ev.request.name = "click"; - srv_ev.request.param_names.push_back("x"); - srv_ev.request.param_names.push_back("y"); + auto srv_ev = std::make_shared(); + srv_ev->name = "click"; + srv_ev->param_names.push_back("x"); + srv_ev->param_names.push_back("y"); - std::stringstream x_str; - x_str << x; - std::stringstream y_str; - y_str << y; + srv_ev->param_values.push_back(std::to_string(x)); + srv_ev->param_values.push_back(std::to_string(y)); - srv_ev.request.param_values.push_back(x_str.str()); - srv_ev.request.param_values.push_back(y_str.str()); + srv_ev->param_names.push_back("type"); + srv_ev->param_values.push_back(click_type); - srv_ev.request.param_names.push_back("type"); - srv_ev.request.param_values.push_back(click_type); - - if (client.call(srv_ev)) - { - std::cout << "Response from server: " << srv_ev.response.msg << std::endl; - } - else - { - std::cout << "Calling raise event failed" << std::endl; - } + // Fire-and-forget: this callback runs inside the executor spin, so we cannot block on the result here. + client->async_send_request(srv_ev); } } -int main(int argc, char **argv) +int main(int argc, char** argv) { - ros::init(argc, argv, "ed_gui"); + rclcpp::init(argc, argv); - ros::NodeHandle nh; - ros::Subscriber sub_image = nh.subscribe("/ed/gui/map_image", 1, imageCallback); - client = nh.serviceClient("/ed/gui/raise_event"); + g_node = rclcpp::Node::make_shared("ed_gui"); + auto sub_image = + g_node->create_subscription("/ed/gui/map_image", 1, imageCallback); + client = g_node->create_client("/ed/gui/raise_event"); click_type = "navigate"; if (argc >= 2) @@ -59,18 +50,18 @@ int main(int argc, char **argv) click_type = argv[1]; } - cv::namedWindow("map", 1); cv::setMouseCallback("map", mouseCallback); - - ros::Rate r(30); - while(ros::ok()) + rclcpp::WallRate r(30); + while (rclcpp::ok()) { - ros::spinOnce(); + rclcpp::spin_some(g_node); r.sleep(); } + rclcpp::shutdown(); + return 0; } diff --git a/test/test_heightmap_triangulation.cpp b/test/test_heightmap_triangulation.cpp index 7ed7c2ed..3bf99200 100644 --- a/test/test_heightmap_triangulation.cpp +++ b/test/test_heightmap_triangulation.cpp @@ -13,7 +13,7 @@ // ---------------------------------------------------------------------------------------------------- -int main(int argc, char **argv) +int main(int argc, char** argv) { if (argc <= 1) { @@ -21,7 +21,7 @@ int main(int argc, char **argv) return 0; } - std::string image_filename = argv[1]; + std::string const image_filename = argv[1]; // Read input image cv::Mat viz = cv::imread(image_filename); @@ -41,13 +41,13 @@ int main(int argc, char **argv) w.setValue("resolution", 1); w.setValue("blockheight", 0); - tue::config::Reader cfg(w.data()); // Wrap config in reader + tue::config::Reader const cfg(w.data()); // Wrap config in reader std::map shape_cache; // necessary for call, not used // Call shape loader. This will generate a mesh from the file std::stringstream error; - geo::ShapePtr shape = ed::models::loadShape("", cfg, shape_cache, error); + geo::ShapePtr const shape = ed::models::loadShape("", cfg, shape_cache, error); if (!shape) { @@ -64,7 +64,7 @@ int main(int argc, char **argv) std::cout << triangles.size() << " triangles" << std::endl; // Visualize triangles - for(std::vector::const_iterator it = triangles.begin(); it != triangles.end(); ++it) + for (std::vector::const_iterator it = triangles.begin(); it != triangles.end(); ++it) { const geo::TriangleI& t = *it; diff --git a/test/test_mask.cpp b/test/test_mask.cpp index 88f182a8..60fb6fd4 100644 --- a/test/test_mask.cpp +++ b/test/test_mask.cpp @@ -8,19 +8,19 @@ int main() ed::ImageMask m(rgb_image.cols, rgb_image.rows); - for(int y = 0; y < 480; ++y) + for (int y = 0; y < 480; ++y) { - for(int x = 0; x < 640; ++x) + for (int x = 0; x < 640; ++x) { m.addPoint(x, y); } } -// cv::Point2i p_min, p_max; -// m.boundingRect(p_min, p_max); -// std::cout << p_min << " - " << p_max << std::endl; + // cv::Point2i p_min, p_max; + // m.boundingRect(p_min, p_max); + // std::cout << p_min << " - " << p_max << std::endl; - int N = 1; + int const N = 1; // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -29,11 +29,11 @@ int main() timer.start(); int i = 0; - for(int n = 0; n < N; ++n) + for (int n = 0; n < N; ++n) { - for(int y = 0; y < rgb_image.rows; ++y) + for (int y = 0; y < rgb_image.rows; ++y) { - for(int x = 0; x < rgb_image.cols; ++x) + for (int x = 0; x < rgb_image.cols; ++x) { i += rgb_image.at(y, x)[0]; } @@ -53,11 +53,11 @@ int main() timer.start(); int i = 0; - for(int n = 0; n < N; ++n) + for (int n = 0; n < N; ++n) { - for(ed::ImageMask::const_iterator it = m.begin(); it != m.end(); ++it) + for (ed::ImageMask::const_iterator it = m.begin(); it != m.end(); ++it) { -// std::cout << *it << std::endl; + // std::cout << *it << std::endl; i += rgb_image.at(it())[0]; } } @@ -66,7 +66,6 @@ int main() std::cout << "Check value: " << i << std::endl; std::cout << timer.getElapsedTimeInMilliSec() / N << " ms" << std::endl; - } return 0; diff --git a/test/test_service_speed.cpp b/test/test_service_speed.cpp index 53dd4dee..91a85614 100644 --- a/test/test_service_speed.cpp +++ b/test/test_service_speed.cpp @@ -1,120 +1,45 @@ -#include -#include -#include -#include -#include -#include +#include -#include +#include +#include +#include +#include +#include #include -int main(int argc, char **argv) { - ros::init(argc, argv, "ed_test_service_speed"); +template void timeService(const rclcpp::Node::SharedPtr& node, const std::string& name, int N) +{ + auto client = node->create_client(name); + client->wait_for_service(); - ros::NodeHandle nh; - - int N = 1; - - { - ros::ServiceClient client = nh.serviceClient("/ed/simple_query"); - client.waitForExistence(); - ed_msgs::SimpleQuery srv; - - tue::Timer t; - t.start(); - - for(int i = 0; i < N; ++i) - { - - if (!client.call(srv)) - { - std::cout << client.getService() << " : could not be called" << std::endl; - } - } - - std::cout << client.getService() << ": " << t.getElapsedTimeInMilliSec() / N << " ms" << std::endl; - } - - { - ros::ServiceClient client = nh.serviceClient("/ed/gui/set_label"); - client.waitForExistence(); - ed_msgs::SetLabel srv; - - tue::Timer t; - t.start(); - - for(int i = 0; i < N; ++i) - { - - if (!client.call(srv)) - { - std::cout << client.getService() << " : could not be called" << std::endl; - } - } - - std::cout << client.getService() << ": " << t.getElapsedTimeInMilliSec() / N << " ms" << std::endl; - } + tue::Timer t; + t.start(); + for (int i = 0; i < N; ++i) { - ros::ServiceClient client = nh.serviceClient("/ed/gui/get_measurements"); - client.waitForExistence(); - ed_msgs::GetMeasurements srv; - - tue::Timer t; - t.start(); - - for(int i = 0; i < N; ++i) - { - - if (!client.call(srv)) - { - std::cout << client.getService() << " : could not be called" << std::endl; - } - } - - std::cout << client.getService() << ": " << t.getElapsedTimeInMilliSec() / N << " ms" << std::endl; + auto request = std::make_shared(); + auto future = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, future) != rclcpp::FutureReturnCode::SUCCESS) + std::cout << name << " : could not be called" << std::endl; } - { - ros::ServiceClient client = nh.serviceClient("/ed/gui/get_gui_command"); - client.waitForExistence(); - ed_msgs::GetGUICommand srv; - - tue::Timer t; - t.start(); - - for(int i = 0; i < N; ++i) - { - - if (!client.call(srv)) - { - std::cout << client.getService() << " : could not be called" << std::endl; - } - } - - std::cout << client.getService() << ": " << t.getElapsedTimeInMilliSec() / N << " ms" << std::endl; - } - - { - ros::ServiceClient client = nh.serviceClient("/ed/gui/raise_event"); - client.waitForExistence(); - ed_msgs::RaiseEvent srv; - - tue::Timer t; - t.start(); + std::cout << name << ": " << t.getElapsedTimeInMilliSec() / N << " ms" << std::endl; +} - for(int i = 0; i < N; ++i) - { +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr const node = rclcpp::Node::make_shared("ed_test_service_speed"); - if (!client.call(srv)) - { - std::cout << client.getService() << " : could not be called" << std::endl; - } - } + int const N = 1; - std::cout << client.getService() << ": " << t.getElapsedTimeInMilliSec() / N << " ms" << std::endl; - } + timeService(node, "/ed/simple_query", N); + timeService(node, "/ed/gui/set_label", N); + timeService(node, "/ed/gui/get_measurements", N); + timeService(node, "/ed/gui/get_gui_command", N); + timeService(node, "/ed/gui/raise_event", N); + rclcpp::shutdown(); return 0; } diff --git a/test/test_wm.cpp b/test/test_wm.cpp index bafa2c9c..d2cb210c 100644 --- a/test/test_wm.cpp +++ b/test/test_wm.cpp @@ -1,8 +1,6 @@ -#include -#include #include - -#include // Why do we need this? +#include +#include // Profiling #include @@ -17,22 +15,22 @@ void buildWorldModel(ed::WorldModel& wm) req.setType("table", "object"); req.setType("bottle", "object"); - boost::shared_ptr t1(new ed::TransformCache()); + boost::shared_ptr const t1(new ed::TransformCache()); t1->insert(0, geo::Pose3D(1, 2, 3, 0, 0, M_PI / 2)); req.setRelation("map", "table", t1); - boost::shared_ptr t2(new ed::TransformCache()); + boost::shared_ptr const t2(new ed::TransformCache()); t2->insert(0, geo::Pose3D(1, 0, 0.75).inverse()); req.setRelation("bottle", "table", t2); std::string parent_id = "bottle"; - for(unsigned int i = 0; i < 100000; ++i) + for (unsigned int i = 0; i < 100000; ++i) { std::stringstream id; id << "e" << i; req.setType(id.str(), "object"); - boost::shared_ptr t1(new ed::TransformCache()); + boost::shared_ptr const t1(new ed::TransformCache()); t1->insert(0, geo::Pose3D(1, 2, 3, 0, 0, M_PI / 2)); req.setRelation(parent_id, id.str(), t1); @@ -46,16 +44,16 @@ void buildWorldModel(ed::WorldModel& wm) void profile(const ed::WorldModel& wm) { - unsigned int N = 1000; + unsigned int const N = 1000; - ed::UUID id1 = "map"; - ed::UUID id2 = "e100"; + ed::UUID const id1 = "map"; + ed::UUID const id2 = "e100"; tue::Timer timer; timer.start(); geo::Pose3D tr; - for(unsigned int i = 0; i < N; ++i) + for (unsigned int i = 0; i < N; ++i) { wm.calculateTransform(id1, id2, 0, tr); } @@ -67,8 +65,8 @@ void profile(const ed::WorldModel& wm) void testCorrectness(const ed::WorldModel& wm) { - ed::UUID id1 = "map"; - ed::UUID id2 = "bottle"; + ed::UUID const id1 = "map"; + ed::UUID const id2 = "bottle"; geo::Pose3D tr; if (wm.calculateTransform(id1, id2, 0, tr)) @@ -79,10 +77,8 @@ void testCorrectness(const ed::WorldModel& wm) // ---------------------------------------------------------------------------------------------------- -int main(int argc, char **argv) +int main(int argc, char** argv) { - ros::Time::init(); // Why do we need this? - ed::WorldModel wm; buildWorldModel(wm); diff --git a/tools/configure.cpp b/tools/configure.cpp index ec8ed1df..15320f98 100644 --- a/tools/configure.cpp +++ b/tools/configure.cpp @@ -1,44 +1,52 @@ -#include -#include -#include -#include - -#include - +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include #include #include #include -#include +#include +#include + +using namespace std::chrono_literals; // ---------------------------------------------------------------------------------------------------- -void usage() +static void usage() { - std::cout << "Usage: configure CONFIG_FILE.yaml/json" << std::endl; + std::cout << "Usage: configure CONFIG_FILE.yaml/json" << '\n'; } // ---------------------------------------------------------------------------------------------------- -int main(int argc, char **argv) +int main(int argc, char** argv) { - std::vector myargv; - ros::removeROSArgs(argc, argv, myargv); + std::vector myargv = rclcpp::init_and_remove_ros_arguments(argc, argv); if (myargv.size() != 2) { usage(); return 1; } - ros::init(argc, argv, "ed_configure"); - - ros::NodeHandle nh; - ros::ServiceClient client = nh.serviceClient("ed/configure"); + rclcpp::Node::SharedPtr const node = rclcpp::Node::make_shared("ed_configure"); + rclcpp::Client::SharedPtr const client = + node->create_client("ed/configure"); - tue::filesystem::Path config_file(myargv[1]); - if (!config_file.exists()) + std::filesystem::path const config_file(myargv[1]); + if (!std::filesystem::exists(config_file)) { - ROS_ERROR_STREAM("Could not configure ED: config file '" << config_file.string() << "' does not exist"); + RCLCPP_ERROR_STREAM(node->get_logger(), + "Could not configure ED: config file '" << config_file.string() << "' does not exist"); return 1; } @@ -49,26 +57,32 @@ int main(int argc, char **argv) tue::Configuration config; if (!tue::config::loadFromYAMLFile(config_file.string(), config, resolve_config)) { - ROS_ERROR_STREAM("Could not configure ED: Error during parsing of the config file '" << config_file.string() << "' "<< std::endl << std::endl << config.error()); + RCLCPP_ERROR_STREAM(node->get_logger(), + "Could not configure ED: Error during parsing of the config file '" << config_file.string() + << "' " << '\n' + << '\n' + << config.error()); return 1; } - ed_msgs::Configure srv; - srv.request.request = config.toYAMLString(); + auto request = std::make_shared(); + request->request = config.toYAMLString(); // We do this as late as possible, so as much time as possible has passed doing other stuff // and we wait as less as possible. - client.waitForExistence(); + client->wait_for_service(); - if (!client.call(srv)) + auto future = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, future) != rclcpp::FutureReturnCode::SUCCESS) { - ROS_ERROR_STREAM("Could not configure ED: Service call failed"); + RCLCPP_ERROR_STREAM(node->get_logger(), "Could not configure ED: Service call failed"); return 1; } - if (!srv.response.error_msg.empty()) + auto response = future.get(); + if (!response->error_msg.empty()) { - ROS_ERROR_STREAM("Could not configure ED:\n\n" + srv.response.error_msg); + RCLCPP_ERROR_STREAM(node->get_logger(), "Could not configure ED:\n\n" + response->error_msg); return 1; } diff --git a/tools/entity-teleop b/tools/entity-teleop index f977d710..a6442924 100755 --- a/tools/entity-teleop +++ b/tools/entity-teleop @@ -1,9 +1,12 @@ -#!/usr/bin/env python -import rospy +#!/usr/bin/env python3 +import sys +import select +import termios +import tty -from ed_msgs.srv import UpdateSrv +import rclpy -import sys, select, termios, tty +from ed_interfaces.srv import UpdateSrv msg = """ Reading from the keyboard and Publishing to Twist! @@ -27,27 +30,28 @@ CTRL-C to quit """ moveBindings = { - '1':(-0.7,0.7,0), - '2':(-1,0,0), - '3':(-0.7,-0.7,0), - '4':(0,1,0), - '6':(0,-1,0), - '7':(0.7,0.7,0), - '8':(1,0,0), - '9':(0.7,-0.7,0), - '/':(0,0,1), - '*':(0,0,-1) + '1': (-0.7, 0.7, 0), + '2': (-1, 0, 0), + '3': (-0.7, -0.7, 0), + '4': (0, 1, 0), + '6': (0, -1, 0), + '7': (0.7, 0.7, 0), + '8': (1, 0, 0), + '9': (0.7, -0.7, 0), + '/': (0, 0, 1), + '*': (0, 0, -1) } -speedBindings={ - 'q':(1.1,1.1), - 'z':(.9,.9), - 'w':(1.1,1), - 'x':(.9,1), - 'e':(1,1.1), - 'c':(1,.9), +speedBindings = { + 'q': (1.1, 1.1), + 'z': (.9, .9), + 'w': (1.1, 1), + 'x': (.9, 1), + 'e': (1, 1.1), + 'c': (1, .9), } + def getKey(): tty.setraw(sys.stdin.fileno()) select.select([sys.stdin], [], [], 0) @@ -55,26 +59,30 @@ def getKey(): termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) return key + speed = .01 turn = .02 -def vels(speed,turn): - return "currently:\tspeed %s\tturn %s " % (speed,turn) -if __name__=="__main__": +def vels(speed, turn): + return "currently:\tspeed %s\tturn %s " % (speed, turn) + + +if __name__ == "__main__": if len(sys.argv) < 2: - print "Please specify entity id" + print("Please specify entity id") exit(1) entity_id = sys.argv[1] settings = termios.tcgetattr(sys.stdin) - rospy.init_node('teleop_twist_keyboard') + rclpy.init() + node = rclpy.create_node('teleop_twist_keyboard') - rospy.wait_for_service('/ed/update') - update = rospy.ServiceProxy('/ed/update', UpdateSrv) + client = node.create_client(UpdateSrv, '/ed/update') + client.wait_for_service() status = 0 @@ -82,17 +90,17 @@ if __name__=="__main__": y = 0 th = 0 - print msg + print(msg) - while(1): + while True: key = getKey() if key in speedBindings.keys(): speed = speed * speedBindings[key][0] turn = turn * speedBindings[key][1] - print vels(speed,turn) - if (status == 14): - print msg + print(vels(speed, turn)) + if status == 14: + print(msg) status = (status + 1) % 15 else: if key in moveBindings.keys(): @@ -103,7 +111,7 @@ if __name__=="__main__": dx = 0 dy = 0 dth = 0 - if (key == '\x03'): + if key == '\x03': break x += dx * speed @@ -113,9 +121,14 @@ if __name__=="__main__": try: s = '{entities: [ { id: ' + entity_id + ', pose: { x: ' + str(x) + ', y: ' + str(y) + ', z: 0, rz: ' + str(th) + ' } } ] }' - resp = update(s) + req = UpdateSrv.Request() + req.request = s + future = client.call_async(req) + rclpy.spin_until_future_complete(node, future) + resp = future.result() if resp.response: - print resp.response - except rospy.ServiceException, e: - print "Service call failed: %s"%e + print(resp.response) + except Exception as e: + print("Service call failed: %s" % e) + rclpy.shutdown() diff --git a/tools/heightmap_to_mesh.cpp b/tools/heightmap_to_mesh.cpp index af892310..af5c6180 100644 --- a/tools/heightmap_to_mesh.cpp +++ b/tools/heightmap_to_mesh.cpp @@ -1,35 +1,47 @@ #include "../src/models/shape_loader_private.h" +#include +#include #include -#include +#include // IWYU pragma: keep // complete geo::Shape type needed for dereference below +#include +#include +#include +#include - -int main(int argc, char **argv) +int main(int argc, char** argv) { // Parse command-line arguments - if (argc < 3 || argc > 7) { - std::cout << "Usage: ed_heightmap_to_mesh INPUT_IMAGE OUTPUT_FILE RESOLUTION [BLOCK_HEIGHT] [ORIGIN_X ORIGIN_Y]" << std::endl; + if (argc < 3 || argc > 7) + { + std::cout << "Usage: ed_heightmap_to_mesh INPUT_IMAGE OUTPUT_FILE RESOLUTION [BLOCK_HEIGHT] [ORIGIN_X ORIGIN_Y]" + << '\n'; return 1; } - std::string input_file = argv[1]; - std::string output_file = argv[2]; + std::string const input_file = argv[1]; + std::string const output_file = argv[2]; double resolution = 0.2; - if (argc > 2) { + if (argc > 2) + { resolution = atof(argv[3]); } double block_height = 1; - if (argc > 3) { + if (argc > 3) + { block_height = atof(argv[4]); } - double origin_x = 0, origin_y = 0; - if (argc > 5) { - if (argc < 7) { - std::cout << "ORIGIN_X and ORIGIN_Y are optional, but shoud be provided together" << std::endl; + double origin_x = 0; + double origin_y = 0; + if (argc > 5) + { + if (argc < 7) + { + std::cout << "ORIGIN_X and ORIGIN_Y are optional, but shoud be provided together" << '\n'; return 1; } origin_x = atof(argv[5]); @@ -38,24 +50,24 @@ int main(int argc, char **argv) // Call shape loader. This will generate a mesh from the file std::stringstream error; - geo::ShapePtr shape = ed::models::getHeightMapShape(input_file, geo::Vec3(origin_x, origin_y, 0), block_height, - resolution, resolution, false, error); + geo::ShapePtr const shape = ed::models::getHeightMapShape( + input_file, geo::Vec3(origin_x, origin_y, 0), block_height, resolution, resolution, false, error); - if(!shape) + if (!shape) { - std::cout << "could not load heightmap: " << input_file << std::endl << error.str() << std::endl; + std::cout << "could not load heightmap: " << input_file << '\n' << error.str() << '\n'; return 1; } if (!geo::io::writeMeshFile(output_file, *shape)) { - std::cout << "Could not convert loaded shape to mesh file: " << output_file << std::endl; + std::cout << "Could not convert loaded shape to mesh file: " << output_file << '\n'; return 1; } - std::cout << "Succesfully converted: '" << input_file << "' to '" << output_file <<"'. With " << - shape->getMesh().getPoints().size() << " points and " << shape->getMesh().getTriangleIs().size() << - " triangles." << std::endl; + std::cout << "Succesfully converted: '" << input_file << "' to '" << output_file << "'. With " + << shape->getMesh().getPoints().size() << " points and " << shape->getMesh().getTriangleIs().size() + << " triangles." << '\n'; return 0; } diff --git a/tools/list_plugins b/tools/list_plugins index 8b734536..01300014 100755 --- a/tools/list_plugins +++ b/tools/list_plugins @@ -1,11 +1,9 @@ -#! /usr/bin/env python -from itertools import chain +#! /usr/bin/env python3 import os.path import sys import xml.etree.ElementTree as ET -import roslib -import rospkg +from ament_index_python.resources import get_resource, get_resources INDENT = " " @@ -14,26 +12,26 @@ def list_plugins(): """ Iterate over all plugin files defining ED plugins. Print the defined libraries and their plugins. """ - rospack = rospkg.RosPack() - to_check = rospack.get_depends_on('ed', implicit=False) - to_check.append("ed") # also check for plugins in ed - - for index, pkg in enumerate(to_check): - m = rospack.get_manifest(pkg) - plugin_file = m.get_export('ed', 'plugin') - if not plugin_file: - continue - elif len(plugin_file) != 1: - print(f"Cannot load plugin [${pkg}]: invalid 'plugin' attribute", file=sys.stderr) - continue + # pluginlib registers the ed plugin description files (via + # pluginlib_export_plugin_description_file(ed ...)) under this resource index. + resources = get_resources("ed__pluginlib__plugin") - plugin_file = plugin_file[0] - if not os.path.isfile(plugin_file): - print(f"Plugin file: '${plugin_file}' is not a file", file=sys.stderr) + for index, pkg in enumerate(sorted(resources)): + content, prefix_path = get_resource("ed__pluginlib__plugin", pkg) + + plugin_files = [rel.strip() for rel in content.splitlines() if rel.strip()] + if not plugin_files: continue - if index: - print("") # Empty line between different files, but not at the end - print_plugin_file(plugin_file) + + for plugin_file in plugin_files: + if not os.path.isabs(plugin_file): + plugin_file = os.path.join(prefix_path, plugin_file) + if not os.path.isfile(plugin_file): + print(f"Plugin file: '{plugin_file}' is not a file", file=sys.stderr) + continue + if index: + print("") # Empty line between different files, but not at the end + print_plugin_file(plugin_file) def print_plugin_file(plugin_file: str): diff --git a/tools/repl.cpp b/tools/repl.cpp index dbc64be7..3676c4bc 100644 --- a/tools/repl.cpp +++ b/tools/repl.cpp @@ -1,29 +1,30 @@ +#include +#include #include #include -#include -#include -static char** my_completion(const char*, int ,int); -char* my_generator(const char*,int); -char * dupstr (char*); -void *xmalloc (int); +static char** my_completion(const char*, int, int); +char* my_generator(const char*, int); +char* dupstr(char*); +void* xmalloc(int); -char* cmd [] ={ "hello", "world", "hell" ,"word", "quit", " " }; +char* cmd[] = {"hello", "world", "hell", "word", "quit", " "}; int main() { - char *buf; + char* buf; rl_attempted_completion_function = my_completion; - while((buf = readline("\n >> "))!=NULL) { - //enable auto-complete - rl_bind_key('\t',rl_complete); + while ((buf = readline("\n >> ")) != NULL) + { + // enable auto-complete + rl_bind_key('\t', rl_complete); - printf("cmd [%s]\n",buf); - if (strcmp(buf,"quit")==0) + printf("cmd [%s]\n", buf); + if (strcmp(buf, "quit") == 0) break; - if (buf[0]!=0) + if (buf[0] != 0) add_history(buf); } @@ -32,59 +33,61 @@ int main() return 0; } -static char** my_completion( const char * text , int start, int end) +static char** my_completion(const char* text, int start, int end) { - char **matches; + char** matches; - matches = (char **)NULL; + matches = (char**)NULL; if (start == 0) - matches = rl_completion_matches ((char*)text, &my_generator); + matches = rl_completion_matches((char*)text, &my_generator); else - rl_bind_key('\t',rl_abort); + rl_bind_key('\t', rl_abort); return (matches); - } char* my_generator(const char* text, int state) { static int list_index, len; - char *name; + char* name; - if (!state) { + if (!state) + { list_index = 0; - len = strlen (text); + len = strlen(text); } - while (name = cmd[list_index]) { + while (name = cmd[list_index]) + { list_index++; - if (strncmp (name, text, len) == 0) + if (strncmp(name, text, len) == 0) return (dupstr(name)); } /* If no names matched, then return NULL. */ - return ((char *)NULL); - + return ((char*)NULL); } -char * dupstr (char* s) { - char *r; +char* dupstr(char* s) +{ + char* r; - r = (char*) xmalloc ((strlen (s) + 1)); - strcpy (r, s); - return (r); + r = (char*)xmalloc((strlen(s) + 1)); + strcpy(r, s); + return (r); } -void * xmalloc (int size) +void* xmalloc(int size) { - void *buf; + void* buf; - buf = malloc (size); - if (!buf) { - fprintf (stderr, "Error: Out of memory. Exiting.'n"); - exit (1); + buf = malloc(size); + if (!buf) + { + fprintf(stderr, "Error: Out of memory. Exiting.'n"); + exit(1); } return buf; diff --git a/tools/view_model.cpp b/tools/view_model.cpp index def74c5c..1292bb6c 100644 --- a/tools/view_model.cpp +++ b/tools/view_model.cpp @@ -1,60 +1,62 @@ -#include -#include -#include +#include +#include +#include #include +#include #include +#include +#include -#include - +#include #include #include -#include +#include +#include +#include +#include #include -#include -#include -#include "tue/config/loaders/sdf.h" -#include "tue/config/loaders/xml.h" -#include "tue/config/loaders/yaml.h" +#include "ed/types.h" +#include +#include +#include -#include - -#include +#include constexpr double CANVAS_WIDTH = 800; constexpr double CANVAS_HEIGHT = 600; -geo::DepthCamera cam; +static geo::DepthCamera cam; -geo::Vector3 cam_lookat; -double cam_dist, cam_yaw, cam_pitch; -cv::Point LAST_MOUSE_POS; -bool do_rotate = true; -geo::Pose3D cam_pose; +static geo::Vector3 cam_lookat; +static double cam_dist, cam_yaw, cam_pitch; +static cv::Point LAST_MOUSE_POS; +static bool do_rotate = true; +static geo::Pose3D cam_pose; -bool do_flyto = false; -geo::Vector3 cam_lookat_flyto; +static bool do_flyto = false; +static geo::Vector3 cam_lookat_flyto; -bool render_required = true; +static bool render_required = true; -cv::Mat depth_image; -cv::Mat image; +static cv::Mat depth_image; +static cv::Mat image; // ---------------------------------------------------------------------------------------------------- -void usage() +static void usage() { - std::cout << "Usage: ed_view_model [ --file | --model ] FILE-OR-MODEL-NAME" << std::endl; + std::cout << "Usage: ed_view_model [ --file | --model ] FILE-OR-MODEL-NAME" << '\n'; } // ---------------------------------------------------------------------------------------------------- -void CallBackFunc(int event, int x, int y, int flags, void* /*userdata*/) +static void CallBackFunc(int event, int x, int y, int flags, void* /*userdata*/) { if (event == cv::EVENT_LBUTTONDBLCLK) { - float d = depth_image.at(y, x); + float const d = depth_image.at(y, x); if (d > 0) { cam_lookat_flyto = cam_pose * (cam.project2Dto3D(x, y) * d); @@ -78,8 +80,8 @@ void CallBackFunc(int event, int x, int y, int flags, void* /*userdata*/) } else if (event == cv::EVENT_MOUSEMOVE) { - double dx = x - LAST_MOUSE_POS.x; - double dy = y - LAST_MOUSE_POS.y; + double const dx = x - LAST_MOUSE_POS.x; + double const dy = y - LAST_MOUSE_POS.y; if (flags & cv::EVENT_FLAG_LBUTTON) { @@ -106,7 +108,7 @@ void CallBackFunc(int event, int x, int y, int flags, void* /*userdata*/) // ---------------------------------------------------------------------------------------------------- -int main(int argc, char **argv) +int main(int argc, char** argv) { if (argc != 3) { @@ -114,7 +116,7 @@ int main(int argc, char **argv) return 1; } - std::string load_type_str = argv[1]; + std::string const load_type_str = argv[1]; ed::models::LoadType load_type; if (load_type_str == "--model") load_type = ed::models::LoadType::MODEL; @@ -122,11 +124,11 @@ int main(int argc, char **argv) load_type = ed::models::LoadType::FILE; else { - std::cerr << "Load type should either be --model or --file" << std::endl; + std::cerr << "Load type should either be --model or --file" << '\n'; usage(); return 1; } - std::string source = argv[2]; + std::string const source = argv[2]; ed::UpdateRequest req; if (!ed::models::loadModel(load_type, source, req)) @@ -137,10 +139,14 @@ int main(int argc, char **argv) world_model.update(req); // Set camera specs - cam = geo::DepthCamera(CANVAS_WIDTH, CANVAS_HEIGHT, - 0.87 * CANVAS_WIDTH, 0.87 * CANVAS_WIDTH, - CANVAS_WIDTH / 2 + 0.5, CANVAS_HEIGHT / 2 + 0.5, - 0, 0); + cam = geo::DepthCamera(CANVAS_WIDTH, + CANVAS_HEIGHT, + 0.87 * CANVAS_WIDTH, + 0.87 * CANVAS_WIDTH, + (CANVAS_WIDTH / 2) + 0.5, + (CANVAS_HEIGHT / 2) + 0.5, + 0, + 0); // Determine min and max coordinates of model geo::Vector3 p_min(1e9, 1e9, 1e9); @@ -149,19 +155,17 @@ int main(int argc, char **argv) int n_vertices = 0; int n_triangles = 0; - for(ed::WorldModel::const_iterator it = world_model.begin(); it != world_model.end(); ++it) + for (const auto& e : world_model) { - const ed::EntityConstPtr& e = *it; - if (e->visual()) { const std::string& id = e->id().str(); if (id.size() < 5 || id.substr(id.size() - 5) != "floor") // Filter ground plane { const std::vector& vertices = e->visual()->getMesh().getPoints(); - for(unsigned int i = 0; i < vertices.size(); ++i) + for (const auto& vertice : vertices) { - const geo::Vector3& p = e->pose() * vertices[i]; + const geo::Vector3& p = e->pose() * vertice; p_min.x = std::min(p.x, p_min.x); p_min.y = std::min(p.y, p_min.y); p_min.z = std::min(p.z, p_min.z); @@ -177,30 +181,30 @@ int main(int argc, char **argv) } } - double dist = 2 * std::max(p_max.z - p_min.z, std::max(p_max.x - p_min.x, p_max.y - p_min.y)); + double const dist = 2 * std::max({p_max.z - p_min.z, p_max.x - p_min.x, p_max.y - p_min.y}); std::stringstream info_msg; - info_msg << "Model loaded successfully:" << std::endl; - info_msg << " " << n_vertices << " vertices" << std::endl; - info_msg << " " << n_triangles << " triangles" << std::endl; - info_msg << " " << "x: [" << p_min.x << " - " << p_max.x << "]" << std::endl; - info_msg << " " << "y: [" << p_min.y << " - " << p_max.y << "]" << std::endl; - info_msg << " " << "z: [" << p_min.z << " - " << p_max.z << "]" << std::endl; - - info_msg << std::endl; - info_msg << "Mouse:" << std::endl; - info_msg << " left - orbit" << std::endl; - info_msg << " middle - zoom" << std::endl; - info_msg << " right - pan" << std::endl; - info_msg << " double click - fly to" << std::endl; - - info_msg << std::endl; - info_msg << "Keys:" << std::endl; - info_msg << " r - reload model" << std::endl; - info_msg << " v - hide all volumes, show model volumes, show room volumes" << std::endl; - info_msg << " c - circle rotate" << std::endl; - info_msg << " p - snap pitch" << std::endl; - info_msg << " q - quit" << std::endl; + info_msg << "Model loaded successfully:" << '\n'; + info_msg << " " << n_vertices << " vertices" << '\n'; + info_msg << " " << n_triangles << " triangles" << '\n'; + info_msg << " " << "x: [" << p_min.x << " - " << p_max.x << "]" << '\n'; + info_msg << " " << "y: [" << p_min.y << " - " << p_max.y << "]" << '\n'; + info_msg << " " << "z: [" << p_min.z << " - " << p_max.z << "]" << '\n'; + + info_msg << '\n'; + info_msg << "Mouse:" << '\n'; + info_msg << " left - orbit" << '\n'; + info_msg << " middle - zoom" << '\n'; + info_msg << " right - pan" << '\n'; + info_msg << " double click - fly to" << '\n'; + + info_msg << '\n'; + info_msg << "Keys:" << '\n'; + info_msg << " r - reload model" << '\n'; + info_msg << " v - hide all volumes, show model volumes, show room volumes" << '\n'; + info_msg << " c - circle rotate" << '\n'; + info_msg << " p - snap pitch" << '\n'; + info_msg << " q - quit" << '\n'; std::cout << info_msg.str(); @@ -211,11 +215,11 @@ int main(int argc, char **argv) cam_yaw = 0; cam_pitch = 0.7; - //Create a window + // Create a window cv::namedWindow("visualization", 1); - //set the callback function for any mouse event - cv::setMouseCallback("visualization", CallBackFunc, NULL); + // set the callback function for any mouse event + cv::setMouseCallback("visualization", CallBackFunc, nullptr); while (true) { @@ -224,9 +228,9 @@ int main(int argc, char **argv) cam_pose.t.z = sin(cam_pitch) * cam_dist; cam_pose.t += cam_lookat; - geo::Vector3 rz = -(cam_lookat - cam_pose.t).normalized(); - geo::Vector3 rx = geo::Vector3(0, 0, 1).cross(rz).normalized(); - geo::Vector3 ry = rz.cross(rx).normalized(); + geo::Vector3 const rz = -(cam_lookat - cam_pose.t).normalized(); + geo::Vector3 const rx = geo::Vector3(0, 0, 1).cross(rz).normalized(); + geo::Vector3 const ry = rz.cross(rx).normalized(); cam_pose.R = geo::Matrix3(rx, ry, rz); @@ -238,13 +242,14 @@ int main(int argc, char **argv) if (render_required) { depth_image = cv::Mat(CANVAS_HEIGHT, CANVAS_WIDTH, CV_32FC1, 0.0); - image = cv::Mat(depth_image.rows, depth_image.cols, CV_8UC3, cv::Scalar(20, 20, 20)); // Not completely black + image = + cv::Mat(depth_image.rows, depth_image.cols, CV_8UC3, cv::Scalar(20, 20, 20)); // Not completely black ed::renderWorldModel(world_model, show_volumes, cam, cam_pose.inverse(), depth_image, image); render_required = false; } cv::imshow("visualization", image); - char key = cv::waitKey(10); + char const key = cv::waitKey(10); if (key == 'r') { @@ -273,9 +278,9 @@ int main(int argc, char **argv) { // Snap pitch to 90 degrees if (cam_pitch < M_PI_2) - cam_pitch = std::round(cam_pitch / M_PI_2 + 0.51) * M_PI_2; + cam_pitch = std::round((cam_pitch / M_PI_2) + 0.51) * M_PI_2; else - cam_pitch = std::round(cam_pitch / M_PI_2 - 0.51) * M_PI_2; + cam_pitch = std::round((cam_pitch / M_PI_2) - 0.51) * M_PI_2; render_required = true; } @@ -288,10 +293,10 @@ int main(int argc, char **argv) if (do_flyto) { - geo::Vector3 diff = cam_lookat_flyto - cam_lookat; - double dist = diff.length(); + geo::Vector3 const diff = cam_lookat_flyto - cam_lookat; + double const dist = diff.length(); - double max_dist = std::max(0.001 * cam_dist, dist * 0.1); + double const max_dist = std::max(0.001 * cam_dist, dist * 0.1); if (dist < max_dist) { cam_lookat = cam_lookat_flyto;