Added new docking

README.md

@@ -10,6 +10,7 @@
 `scitos2` is a ROS 2 stack designed for Metralabs robots that utilize the MIRA framework, including models such as SCITOS, TORY, MORPHIA, etc. This stack comprises several packages, each serving a unique purpose:
 
  * [scitos2_behavior_tree]: This package contains behavior tree nodes that extend your robot's functionalities, such as emergency stop, reset motor stop, etc.
+ * [scito2_charing_dock]: This package contains the implementation of the charging dock plugin for the SCITOS and TORY robots from MetraLabs using the opennav_docking server.
  * [scitos2_common]: This package provides common functionalities for the scitos2 stack.
  * [scitos2_core]: This package provides the abstract interface (virtual base classes) for the Scitos Modules.
  * [scitos2_mira]: This is the main node that interfaces with the MIRA framework.
@@ -36,9 +37,8 @@ rosdep install -i --from-path src --rosdistro rolling -y
 colcon build --symlink-install
 ```
 
-[Ubuntu]: https://ubuntu.com/
-[ROS2]: https://docs.ros.org/en/rolling/
 [scitos2_behavior_tree]: /scitos2_behavior_tree
+[scito2_charing_dock]: /scitos2_charging_dock
 [scitos2_common]: /scitos2_common
 [scitos2_core]: /scitos2_core
 [scitos2_mira]: /scitos2_mira

scitos2/package.xml

@@ -9,6 +9,7 @@
   <author email="ajtudela@gmail.com">Alberto Tudela</author>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <exec_depend>scitos2_behavior_tree</exec_depend>
+  <exec_depend>scitos2_charging_dock</exec_depend>
   <exec_depend>scitos2_core</exec_depend>
   <exec_depend>scitos2_mira</exec_depend>
   <exec_depend>scitos2_modules</exec_depend>

scitos2_behavior_tree/CHANGELOG.rst

@@ -1,6 +1,6 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package scitos2_behavior_tree
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 2.0.0 (XX-04-2024)
 ------------------

scitos2_behavior_tree/src/generate_scitos2_tree_nodes_xml.cpp

@@ -24,6 +24,7 @@
 
 #include "plugins_list.hpp"
 
+// LCOV_EXCL_START
 int main()
 {
   BT::BehaviorTreeFactory factory;
@@ -45,3 +46,4 @@ int main()
 
   return 0;
 }
+// LCOV_EXCL_STOP

scitos2_charging_dock/CHANGELOG.rst

@@ -0,0 +1,10 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package scitos2_charging_dock
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+3.0.0 (XX-XX-XXXX)
+------------------
+* Initial release.
+* Create README.md.
+* Redo the package to use the new docking system.
+* Contributors: Alberto Tudela

scitos2_charging_dock/CMakeLists.txt

@@ -0,0 +1,154 @@
+cmake_minimum_required(VERSION 3.5)
+project(scitos2_charging_dock)
+
+if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
+  message(STATUS "Setting build type to Release as none was specified.")
+  set(CMAKE_BUILD_TYPE "Release" CACHE
+    STRING "Choose the type of build." FORCE)
+
+  # Set the possible values of build type for cmake-gui
+  set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS
+    "Debug" "Release" "MinSizeRel" "RelWithDebInfo")
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  if("cxx_std_17" IN_LIST CMAKE_CXX_COMPILE_FEATURES)
+    set(CMAKE_CXX_STANDARD 17)
+  else()
+    message(FATAL_ERROR "cxx_std_17 could not be found.")
+  endif()
+endif()
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference)
+  add_compile_options("$<$<COMPILE_LANGUAGE:CXX>:-Wnon-virtual-dtor>")
+endif()
+
+option(COVERAGE_ENABLED "Enable code coverage" FALSE)
+
+if(COVERAGE_ENABLED)
+  add_compile_options(--coverage)
+  set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --coverage")
+  set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} --coverage")
+endif()
+
+# Defaults for Microsoft C++ compiler
+if(MSVC)
+  # https://blog.kitware.com/create-dlls-on-windows-without-declspec-using-new-cmake-export-all-feature/
+  set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+
+  # Enable Math Constants
+  # https://docs.microsoft.com/en-us/cpp/c-runtime-library/math-constants?view=vs-2019
+  add_compile_definitions(
+    _USE_MATH_DEFINES
+  )
+endif()
+
+# ###############################################
+# # Find  dependencies                         ##
+# ###############################################
+# # Find ament macros and libraries
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(scitos2_msgs REQUIRED)
+find_package(opennav_docking_core REQUIRED)
+find_package(opennav_docking REQUIRED)
+
+# ##########
+# # Build ##
+# ##########
+# # Specify additional locations of header files
+# # Your package locations should be listed before other locations
+include_directories(
+  include
+)
+
+set(dependencies
+  rclcpp
+  rclcpp_components
+  pluginlib
+  geometry_msgs
+  sensor_msgs
+  nav2_util
+  pcl_ros
+  pcl_conversions
+  tf2_ros
+  scitos2_msgs
+  opennav_docking_core
+  opennav_docking
+)
+
+set(library_name ${PROJECT_NAME}_core)
+set(dock_saver_executable dock_saver)
+
+# Add library
+add_library(${library_name} SHARED
+  src/segmentation.cpp
+  src/perception.cpp
+  src/charging_dock.cpp
+  src/dock_saver.cpp
+)
+ament_target_dependencies(${library_name} ${dependencies})
+
+pluginlib_export_plugin_description_file(opennav_docking_core plugins.xml)
+
+# Add dock saver executable
+add_executable(${dock_saver_executable} src/main_saver.cpp)
+ament_target_dependencies(${dock_saver_executable} ${dependencies})
+target_link_libraries(${dock_saver_executable} ${library_name})
+
+rclcpp_components_register_nodes(${library_name} "scitos2_charging_dock::DockSaver")
+
+# ############
+# # Install ##
+# ############
+install(TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS ${dock_saver_executable}
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(DIRECTORY launch params
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(FILES test/dock_test.pcd test/empty_dock_test.pcd
+  DESTINATION share/${PROJECT_NAME}/test
+)
+
+# ############
+# # Testing ##
+# ############
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+  add_subdirectory(test)
+endif()
+
+# ##################################
+# # ament specific configuration  ##
+# ##################################
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME} ${library_name})
+ament_export_dependencies(${dependencies})
+ament_package()

scitos2_charging_dock/README.md

@@ -0,0 +1,139 @@
+# scitos2_charging_dock
+
+## Overview
+
+This package contains the implementation of the charging dock plugin for the SCITOS and TORY robots from MetraLabs using **[opennav_docking] server**.
+
+The plugin is responsible for detecting the charging dock and obtaining the final refined pose of the dock in the robot's frame. It uses the Iterative Closest Point (ICP) algorithm to align the template of the charging dock (previously recorded) to the current pointcloud of the dock. The plugin also uses the battery state of the robot to determine when to stop the docking process.
+
+A **save_dock** service is provided to save the current pointcloud of the charging dock as the template for future matching.
+
+## Charging Dock plugin
+
+### Subscribed Topics
+
+* **`scan`** ([sensor_msgs/LaserScan])
+
+	Topic where the laser scan data is published.
+
+* **`battery`** ([sensor_msgs/BatteryState])
+
+	Battery state of the robot.
+
+### Published Topics
+
+* **`dock/cloud`** ([sensor_msgs/PointCloud2])
+
+	Pointcloud of the charging station extracted from the laser scan data. This can be enable using *debug* parameter.
+
+* **`dock/template`** ([sensor_msgs/PointCloud2])
+
+	Pointcloud of the recorded charging station used for matching. This can be enable using *debug* parameter.
+
+* **`dock/target`** ([sensor_msgs/PointCloud2])
+
+	Pointcloud of the current cluster used in the current matching. This can be enable using *debug* parameter.
+
+### Parameters
+
+* **`docking_threshold`** (double, default: 0.05)
+
+	The pose threshold to the docking pose where `isDocked() = true`.
+
+* **`staging_x_offset`** (double, default: -0.7)
+
+	Staging pose offset forward (negative) of dock pose (m).
+
+* **`staging_yaw_offset`** (double, default: 0.0)
+
+	Staging pose angle relative to dock pose (rad).
+
+* **`external_detection_timeout`** (double, default: 1.0)
+
+	Timeout at which if the newest detection update does not meet to fail.
+
+* **`external_detection_translation_x`** (double, default: -0.20)
+
+	X offset from detected pose for docking pose (m).
+
+* **`external_detection_translation_y`** (double, default: 0.0)
+
+	Y offset from detected pose for docking pose (m).
+
+* **`external_detection_rotation_roll`** (double, default: -1.57)
+
+	Roll offset from detected pose for docking pose (rad).
+
+* **`external_detection_rotation_pitch`** (double, default: 1.57)
+
+	Pitch offset from detected pose for docking pose (rad).
+
+* **`external_detection_rotation_yaw`** (double, default: 0.0)
+
+	Yaw offset from detected pose for docking pose (rad).
+
+* **`filter_coef`** (double, default: 0.1)
+
+	Dock external detection method filtering algorithm coefficient.
+
+* **`perception.debug`** (bool, default: false)
+
+	Option to visualize the current point clouds used in ICP matching. 
+
+* **`perception.icp_min_score`** (double, default: 0.01)
+
+	ICP Fitness Score Threshold.
+
+* **`perception.icp_max_iter`** (int, default: 200)
+
+	Max number of iterations to fit template cloud to the target cloud.
+
+* **`perception.icp_max_corr_dis`** (double, default: 1.0)
+
+	Max allowable distance for matches in meters.
+
+* **`perception.icp_max_trans_eps`** (double, default: 1.0e-8)
+
+	Max allowable translation squared difference between two consecutive transformations.
+
+* **`perception.icp_max_eucl_fit_eps`** (double, default: 1.0e-8)
+
+	Maximum allowed Euclidean error between two consecutive steps in the ICP loop.
+
+* **`perception.dock_template`** (string, default: "")
+
+	Path to the pointcloud file of the charging station used for matching.
+
+* **`perception.segmentation.distance_threshold`** (double, default: 0.04)
+
+	The maximum distance between points in a cluster.
+
+* **`perception.segmentation.min_points`** (int, default: 25)
+
+	The minimum number of points required for a cluster to be considered valid.
+
+* **`perception.segmentation.max_points`** (int, default: 400)
+
+	The maximum number of points allowed in a cluster.
+
+* **`perception.segmentation.min_distance`** (double, default: 0.0)
+
+	The minimum distance from the sensor to a point in a cluster.
+
+* **`perception.segmentation.max_distance`** (double, default: 2.0)
+
+	The maximum distance from the sensor to a point in a cluster.
+
+* **`perception.segmentation.min_width`** (double, default: 0.3)
+
+	The minimum width of a cluster.
+
+* **`perception.segmentation.max_width`** (double, default: 1.0)
+
+	The maximum width of a cluster.
+
+
+[opennav_docking]: https://github.com/open-navigation/opennav_docking
+[sensor_msgs/LaserScan]: https://docs.ros2.org/humble/api/sensor_msgs/msg/LaserScan.html
+[sensor_msgs/BatteryState]: https://docs.ros2.org/humble/api/sensor_msgs/msg/BatteryState.html
+[sensor_msgs/PointCloud2]: https://docs.ros2.org/humble/api/sensor_msgs/msg/PointCloud2.html