diff --git a/README.md b/README.md index d066897..51b43f2 100644 --- a/README.md +++ b/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 diff --git a/scitos2/package.xml b/scitos2/package.xml index a26eaae..8a14daf 100644 --- a/scitos2/package.xml +++ b/scitos2/package.xml @@ -9,6 +9,7 @@ Alberto Tudela ament_cmake scitos2_behavior_tree + scitos2_charging_dock scitos2_core scitos2_mira scitos2_modules diff --git a/scitos2_behavior_tree/CHANGELOG.rst b/scitos2_behavior_tree/CHANGELOG.rst index 86d4a4c..a0915cc 100644 --- a/scitos2_behavior_tree/CHANGELOG.rst +++ b/scitos2_behavior_tree/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package scitos2_behavior_tree -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2.0.0 (XX-04-2024) ------------------ diff --git a/scitos2_behavior_tree/src/generate_scitos2_tree_nodes_xml.cpp b/scitos2_behavior_tree/src/generate_scitos2_tree_nodes_xml.cpp index 7ebba1b..e381d22 100644 --- a/scitos2_behavior_tree/src/generate_scitos2_tree_nodes_xml.cpp +++ b/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 diff --git a/scitos2_charging_dock/CHANGELOG.rst b/scitos2_charging_dock/CHANGELOG.rst new file mode 100644 index 0000000..1692f09 --- /dev/null +++ b/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 diff --git a/scitos2_charging_dock/CMakeLists.txt b/scitos2_charging_dock/CMakeLists.txt new file mode 100644 index 0000000..df72219 --- /dev/null +++ b/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("$<$:-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() diff --git a/scitos2_charging_dock/README.md b/scitos2_charging_dock/README.md new file mode 100644 index 0000000..a2e0905 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/scitos2_charging_dock/include/scitos2_charging_dock/charging_dock.hpp b/scitos2_charging_dock/include/scitos2_charging_dock/charging_dock.hpp new file mode 100644 index 0000000..7341c25 --- /dev/null +++ b/scitos2_charging_dock/include/scitos2_charging_dock/charging_dock.hpp @@ -0,0 +1,147 @@ +// Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCITOS2_CHARGING_DOCK__CHARGING_DOCK_HPP_ +#define SCITOS2_CHARGING_DOCK__CHARGING_DOCK_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" +#include "opennav_docking_core/charging_dock.hpp" +#include "opennav_docking/pose_filter.hpp" +#include "scitos2_charging_dock/perception.hpp" + +namespace scitos2_charging_dock +{ + +class ChargingDock : public opennav_docking_core::ChargingDock +{ +public: + /** + * @brief Constructor + */ + ChargingDock() + : opennav_docking_core::ChargingDock() + {} + + /** + * @param parent pointer to user's node + * @param name The name of this planner + * @param tf A pointer to a TF buffer + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf); + + /** + * @brief Method to cleanup resources used on shutdown. + */ + virtual void cleanup() {} + + /** + * @brief Method to active Behavior and any threads involved in execution. + */ + virtual void activate() {} + + /** + * @brief Method to deactive Behavior and any threads involved in execution. + */ + virtual void deactivate() {} + + /** + * @brief Method to obtain the dock's staging pose. This method should likely + * be using TF and the dock's pose information to find the staging pose from + * a static or parameterized staging pose relative to the docking pose + * @param pose Dock with pose + * @param frame Dock's frame of pose + * @return PoseStamped of staging pose in the specified frame + */ + virtual geometry_msgs::msg::PoseStamped getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame); + + /** + * @brief Method to obtain the refined pose of the dock, based on sensor + * @param pose The initial estimate of the dock pose. + * @param frame The frame of the initial estimate. + */ + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose); + + /** + * @copydoc opennav_docking_core::ChargingDock::isCharging + */ + virtual bool isCharging(); + + /** + * @copydoc opennav_docking_core::ChargingDock::isDocked + */ + virtual bool isDocked(); + + /** + * @copydoc opennav_docking_core::ChargingDock::disableCharging + */ + virtual bool disableCharging(); + + /** + * @brief Similar to isCharging() but called when undocking. + */ + virtual bool hasStoppedCharging(); + +protected: + // Subscribe to a scan topic + rclcpp::Subscription::SharedPtr scan_sub_; + rclcpp::Publisher::SharedPtr dock_pose_pub_; + rclcpp::Publisher::SharedPtr filtered_dock_pose_pub_; + rclcpp::Publisher::SharedPtr staging_pose_pub_; + // It will contain latest message + sensor_msgs::msg::LaserScan scan_; + // This is the actual dock pose once it has the specified translation/rotation applied + geometry_msgs::msg::PoseStamped dock_pose_; + + // Subscribe to battery message, used to determine if charging + rclcpp::Subscription::SharedPtr battery_sub_; + bool is_charging_; + + // An external reference (sensor_msgs::LaserScan) is used to detect dock + double external_detection_timeout_; + tf2::Quaternion external_detection_rotation_; + double external_detection_translation_x_; + double external_detection_translation_y_; + + // Filtering of detected poses + std::shared_ptr filter_; + + // external pose reference, this is the distance threshold + double docking_threshold_; + // Offset for staging pose relative to dock pose + double staging_x_offset_; + double staging_yaw_offset_; + + // Perception + std::unique_ptr perception_; + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::shared_ptr tf2_buffer_; +}; + +} // namespace scitos2_charging_dock + +#endif // SCITOS2_CHARGING_DOCK__CHARGING_DOCK_HPP_ diff --git a/scitos2_charging_dock/include/scitos2_charging_dock/cluster.hpp b/scitos2_charging_dock/include/scitos2_charging_dock/cluster.hpp new file mode 100644 index 0000000..539ddaf --- /dev/null +++ b/scitos2_charging_dock/include/scitos2_charging_dock/cluster.hpp @@ -0,0 +1,109 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCITOS2_CHARGING_DOCK__CLUSTER_HPP_ +#define SCITOS2_CHARGING_DOCK__CLUSTER_HPP_ + +// PCL +#include +#include +#include + +// C++ +#include + +// ROS +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace scitos2_charging_dock +{ + +/** + * @class scitos2_charging_dock::Cluster + * @brief Class to represent a cluster of points. + */ +struct Cluster +{ + // Identifier of the cluster + int id; + // Original pointcloud of the cluster. + pcl::PointCloud::Ptr cloud; + // Pointcloud of the cluster after perform ICP. + pcl::PointCloud::Ptr matched_cloud; + // Score of the ICP. + double icp_score; + // Pose of the dock. + geometry_msgs::msg::PoseStamped icp_pose; + + /** + * @brief Get the centroid of the dock. + * + * @return geometry_msgs::msg::Point The centroid of the dock. + */ + geometry_msgs::msg::Point getCentroid() + { + Eigen::Vector4f centroid_vec_4f(0, 0, 0, 0); + pcl::compute3DCentroid(*cloud, centroid_vec_4f); + geometry_msgs::msg::Point centroid; + centroid.x = static_cast(centroid_vec_4f[0]); + centroid.y = static_cast(centroid_vec_4f[1]); + centroid.z = static_cast(centroid_vec_4f[2]); + return centroid; + } + + /** + * @brief Get the width of the cluster. + * + * @return double The width of the cluster. + */ + double width() const + { + if (!cloud) { + return 0.0; + } + + double dx = cloud->back().x - cloud->front().x; + double dy = cloud->back().y - cloud->front().y; + return std::hypot(dx, dy); + } + + /** + * @brief Check if the cluster is valid. + * + * @param ideal_size Size of the cluster. + * @return bool If the cluster is valid. + */ + bool valid(double ideal_size) const + { + // If there are no points this cannot be valid. + if (!cloud) { + return false; + } + + // Check overall size. + if (width() > 1.25 * ideal_size || width() < ideal_size / 2.0) { + return false; + } + + return true; + } + + friend bool operator<(const Cluster c1, const Cluster c2) {return c1.icp_score < c2.icp_score;} + friend bool operator>(const Cluster c1, const Cluster c2) {return c1.icp_score > c2.icp_score;} +}; +} // namespace scitos2_charging_dock + +#endif // SCITOS2_CHARGING_DOCK__CLUSTER_HPP_ diff --git a/scitos2_charging_dock/include/scitos2_charging_dock/dock_saver.hpp b/scitos2_charging_dock/include/scitos2_charging_dock/dock_saver.hpp new file mode 100644 index 0000000..bc112e5 --- /dev/null +++ b/scitos2_charging_dock/include/scitos2_charging_dock/dock_saver.hpp @@ -0,0 +1,113 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCITOS2_CHARGING_DOCK__DOCK_SAVER_HPP_ +#define SCITOS2_CHARGING_DOCK__DOCK_SAVER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/buffer.h" +#include "scitos2_msgs/srv/save_dock.hpp" +#include "scitos2_charging_dock/perception.hpp" + +namespace scitos2_charging_dock +{ + +/** + * @class scitos2_charging_dock::DockSaver + * @brief A class that provides dock saving methods and services + */ +class DockSaver : public nav2_util::LifecycleNode +{ +public: + /** + * @brief Constructor for the scitos2_charging_dock::DockSaver + * @param options Additional options to control creation of the node. + */ + explicit DockSaver(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /** + * @brief Destructor for the scitos2_charging_dock::DockServer + */ + ~DockSaver(); + + /** + * @brief Sets up dock saving service + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Called when node switched to active state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Called when node switched to inactive state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Called when it is required node clean-up + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Called when in Shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Callback for saving the dock to a PCD file. + * + * @param request SaveDock service request + * @param response SaveDock service response + * @return bool True if the dock was saved successfully + */ + bool saveDockCallback( + const std::shared_ptr request, + std::shared_ptr response); + +protected: + // The timeout for saving the dock in service + std::shared_ptr save_dock_timeout_; + + // Perception module to interact with scans and point clouds + std::shared_ptr perception_; + + // The TF buffer + std::shared_ptr tf2_buffer_; + + // The name of the service for saving a dock from topic + const std::string save_dock_service_name_{"save_dock"}; + // A service to save the dock to a file at run time (SaveDock) + rclcpp::Service::SharedPtr save_dock_service_; +}; + +} // namespace scitos2_charging_dock + +#endif // SCITOS2_CHARGING_DOCK__DOCK_SAVER_HPP_ diff --git a/scitos2_charging_dock/include/scitos2_charging_dock/perception.hpp b/scitos2_charging_dock/include/scitos2_charging_dock/perception.hpp new file mode 100644 index 0000000..b3ca344 --- /dev/null +++ b/scitos2_charging_dock/include/scitos2_charging_dock/perception.hpp @@ -0,0 +1,201 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCITOS2_CHARGING_DOCK__PERCEPTION_HPP_ +#define SCITOS2_CHARGING_DOCK__PERCEPTION_HPP_ + +// PCL +#include + +// C++ +#include +#include +#include +#include + +// ROS +#include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "tf2/LinearMath/Transform.h" +#include "tf2_ros/buffer.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "scitos2_charging_dock/cluster.hpp" +#include "scitos2_charging_dock/segmentation.hpp" + +namespace scitos2_charging_dock +{ + +using Clusters = std::vector; +using Pcloud = pcl::PointCloud; + +/** + * @class scitos2_charging_dock::Perception + * @brief Class to perform ICP matching on clusters to get the docking station. + * + */ +class Perception +{ +public: + /** + * @brief Create a perception instance. Configure ROS 2 parameters. + * + * @param node The ROS 2 node + * @param name The name of the perception + * @param tf The tf buffer + */ + Perception( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & name, std::shared_ptr tf); + + /** + * @brief Destroy the Perception object + */ + ~Perception(); + + /** + * @brief Store the dock pointcloud to a PCD file. + * + * @param filepath The path to the file + * @param dock The dock to store + * @return bool If the file was stored + */ + bool storeDockPointcloud(std::string filepath, const Pcloud & dock); + + /** + * @brief Get the dock pose from the scan. + * + * @param scan The scan to process + * @return geometry_msgs::msg::PoseStamped The dock pose + */ + geometry_msgs::msg::PoseStamped getDockPose(const sensor_msgs::msg::LaserScan & scan); + + /** + * @brief Set the initial estimate of the dock pose. + * + * @param pose The initial estimate of the dock pose + * @param frame The frame of the pose + */ + void setInitialEstimate(const geometry_msgs::msg::Pose & pose, const std::string & frame); + + /** + * @brief Extract clusters from a scan. + * + * @param scan The scan to process + * @return Clusters The clusters + */ + Clusters extractClustersFromScan(const sensor_msgs::msg::LaserScan & scan); + +protected: + /** + * @brief Convert the segments to clusters. + * + * @param frame The frame of the segments + * @param segments The segments to convert + * @return Clusters The clusters + */ + Clusters segmentsToClusters(std::string frame, const Segments & segments); + + /** + * @brief Convert the segment to a pointcloud. + * + * @param frame The frame of the segment + * @param segments The segments to convert + * @return Pcloud The pointcloud + */ + Pcloud::Ptr segmentToPcloud(std::string frame, const Segment & segment); + + /** + * @brief Refine the cluster pose using Iterative Closest Point. + * + * @param cluster The cluster to perform ICP on + * @param cloud_template The template to match with the cluster + * @return bool If the ICP was successful and the template is aligned with the cluster + */ + bool refineClusterPose(Cluster & cluster, Pcloud::ConstPtr cloud_template); + + /** + * @brief Refine all the clusters poses using Iterative Closest Point to find the dock. + * + * @param clusters The clusters to perform ICP on + * @param cloud_template The template to match with the clusters + * @param dock_pose The dock pose + * @return bool If the dock is found + */ + bool refineAllClustersPoses( + Clusters & clusters, Pcloud::ConstPtr cloud_template, + geometry_msgs::msg::PoseStamped & dock_pose); + + /** + * @brief Load the dock template from a PCD file. + * + * @param filepath The path to the file + * @param dock The loaded dock + * @return bool If the file was loaded + */ + bool loadDockPointcloud(std::string filepath, Pcloud & dock); + + /** + * @brief Convert a Eigen matrix to a tf2 Transform. + * + * @param T The Eigen matrix + * @return tf2::Transform The transform + */ + tf2::Transform eigenToTransform(const Eigen::Matrix4f & T); + + /** + * @brief Callback executed when a parameter change is detected. + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Debug flag for visualization + bool debug_; + // ICP parameters + int icp_max_iter_; + double icp_min_score_; + double icp_max_corr_dis_; + double icp_max_trans_eps_; + double icp_max_eucl_fit_eps_; + // Initial estimate of the dock pose + geometry_msgs::msg::PoseStamped initial_estimate_pose_; + // Segmentation + std::unique_ptr segmentation_; + + // Last detected dock pose + geometry_msgs::msg::PoseStamped detected_dock_pose_; + // Dock template pointcloud + Pcloud::Ptr dock_template_; + + std::string name_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("DockingPerception")}; + std::shared_ptr tf_buffer_; + + // Publishers for the dock cloud and dock template + rclcpp::Publisher::SharedPtr target_cloud_pub_; + rclcpp::Publisher::SharedPtr dock_cloud_pub_; + rclcpp::Publisher::SharedPtr dock_template_pub_; + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::mutex dynamic_params_lock_; +}; + +} // namespace scitos2_charging_dock + +#endif // SCITOS2_CHARGING_DOCK__PERCEPTION_HPP_ diff --git a/scitos2_charging_dock/include/scitos2_charging_dock/segment.hpp b/scitos2_charging_dock/include/scitos2_charging_dock/segment.hpp new file mode 100644 index 0000000..1130f19 --- /dev/null +++ b/scitos2_charging_dock/include/scitos2_charging_dock/segment.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2017 Alberto J. Tudela Roldán +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCITOS2_CHARGING_DOCK__SEGMENT_HPP_ +#define SCITOS2_CHARGING_DOCK__SEGMENT_HPP_ + +// C++ +#include +#include + +// ROS +#include "geometry_msgs/msg/point.hpp" + +namespace scitos2_charging_dock +{ + +/** + * @struct scitos2_charging_dock::Segment + * @brief Struct to store a segment of points. + */ +struct Segment +{ + /** + * @brief Get the size of the segment. + * @return int The size of the segment. + */ + int size() const {return points.size();} + + /** + * @brief Clear the segment. + */ + void clear() {points.clear();} + + /** + * @brief Get the width squared of the segment. + * @return double The width squared of the segment. + */ + double width_squared() const + { + double dx = points.back().x - points.front().x; + double dy = points.back().y - points.front().y; + return dx * dx + dy * dy; + } + + /** + * @brief Get the centroid of the segment. + * + * @return geometry_msgs::msg::Point The centroid of the segment. + */ + geometry_msgs::msg::Point centroid() const + { + geometry_msgs::msg::Point centroid; + for (const auto & point : points) { + centroid.x += point.x; + centroid.y += point.y; + centroid.z += point.z; + } + centroid.x /= points.size(); + centroid.y /= points.size(); + centroid.z /= points.size(); + return centroid; + } + + /** + * @brief Get the length of the centroid. + * + * @return double The length of the centroid. + */ + double centroid_length() const + { + auto c = centroid(); + return sqrt(c.x * c.x + c.y * c.y); + } + + std::vector points; +}; + +} // namespace scitos2_charging_dock + +#endif // SCITOS2_CHARGING_DOCK__SEGMENT_HPP_ diff --git a/scitos2_charging_dock/include/scitos2_charging_dock/segmentation.hpp b/scitos2_charging_dock/include/scitos2_charging_dock/segmentation.hpp new file mode 100644 index 0000000..fb70655 --- /dev/null +++ b/scitos2_charging_dock/include/scitos2_charging_dock/segmentation.hpp @@ -0,0 +1,144 @@ +// Copyright (c) 2017 Alberto J. Tudela Roldán +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCITOS2_CHARGING_DOCK__SEGMENTATION_HPP_ +#define SCITOS2_CHARGING_DOCK__SEGMENTATION_HPP_ + +// C++ +#include +#include + +// ROS +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "scitos2_charging_dock/segment.hpp" + +namespace scitos2_charging_dock +{ + +using Segments = std::vector; + +/** + * @class scitos2_charging_dock::Segmentation + * @brief Class to perform segmentation on the laserscan to get the segments. + * + */ +class Segmentation +{ +public: + /** + * @brief Create a segmentation instance. Configure ROS 2 parameters. + * + * @param node The ROS 2 node + * @param name The name of the segmentation + */ + explicit Segmentation( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & name); + + /** + * @brief Destroy the Segmentation object + */ + ~Segmentation() = default; + + /** + * @brief Perform a segmentation using a euclidean distance base clustering. + * + * @param scan The laserscan to segment + * @param segments The segmented points + * @return bool If the segmentation was successful + */ + bool performSegmentation(const sensor_msgs::msg::LaserScan & scan, Segments & segments); + + /** + * @brief Filter the segments based on the number of points, the distance between them. + * + * @param segments The segments to filter + * @return Segments The filtered segments + */ + Segments filterSegments(const Segments & segments); + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + +protected: + /** + * @brief Convert a polar point to a cartesian point. + * + * @param range The range of the point + * @param angle The angle of the point + * @return geometry_msgs::msg::Point The cartesian point + */ + geometry_msgs::msg::Point fromPolarToCartesian(double range, double angle); + + /** + * @brief Convert a laserscan to a vector of points. + * + * @param scan The laserscan to convert + * @return std::vector The points + */ + std::vector scanToPoints(const sensor_msgs::msg::LaserScan & scan); + + /** + * @brief Calculate the euclidean distance between two points. + * + * @param p1 The first point + * @param p2 The second point + * @return double The distance + */ + double euclideanDistance( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); + + /** + * @brief Check if there is a jump between two points. + * + * @param p1 The first point + * @param p2 The second point + * @param threshold The threshold to consider a jump + * @return bool If there is a jump + */ + bool isJumpBetweenPoints( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, double threshold); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::mutex dynamic_params_lock_; + + // Name of the segmentation + std::string name_; + + // Distance threshold for the segmentation + double distance_threshold_; + // Minimum number of points in a segment + int min_points_segment_; + // Maximum number of points in a segment + int max_points_segment_; + // Minimum distance from sensor to the centroid of the segment + double min_avg_distance_from_sensor_; + // Maximum distance from sensor to the centroid of the segment + double max_avg_distance_from_sensor_; + // Minimum width of the segment + double min_segment_width_; + // Maximum width of the segment + double max_segment_width_; +}; + +} // namespace scitos2_charging_dock + +#endif // SCITOS2_CHARGING_DOCK__SEGMENTATION_HPP_ diff --git a/scitos2_charging_dock/launch/dock_saver.launch.py b/scitos2_charging_dock/launch/dock_saver.launch.py new file mode 100644 index 0000000..3ed1049 --- /dev/null +++ b/scitos2_charging_dock/launch/dock_saver.launch.py @@ -0,0 +1,144 @@ +# Copyright (c) 2024 Alberto J. Tudela Roldán +# Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http:#www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launches a Scitos MIRA node.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + # Create the launch configuration variables. + namespace = LaunchConfiguration('namespace') + autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['dock_saver'] + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) + + # Map these variables to arguments: can be set from the command line or a default will be used + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace' + ) + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='False', + description='Use composed bringup if True', + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', + default_value='info', + description='log level' + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='scitos2_charging_dock', + executable='dock_saver', + name='dock_saver', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + arguments=['--ros-args', '--log-level', log_level] + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_scitos', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ], + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + LoadComposableNodes( + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='scitos2_charging_dock', + plugin='scitos2_charging_dock::DockSaver', + name='dock_saver', + ), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_scitos', + parameters=[ + {'autostart': autostart, 'node_names': lifecycle_nodes} + ], + ), + ], + ), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/scitos2_charging_dock/launch/docking_server.launch.py b/scitos2_charging_dock/launch/docking_server.launch.py new file mode 100644 index 0000000..8f192c2 --- /dev/null +++ b/scitos2_charging_dock/launch/docking_server.launch.py @@ -0,0 +1,180 @@ +# Copyright (c) 2024 Alberto J. Tudela Roldán +# Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http:#www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launches a docking server node.""" + +import os + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node +from launch_ros.descriptions import ComposableNode +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + + # Default filenames and where to find them + scitos_dir = get_package_share_directory('scitos2_charging_dock') + + # Read the YAML parameters file. + default_params_file = os.path.join(scitos_dir, 'params', 'example.yaml') + # This must be changed to the correct path of your docking template file + default_template_file = os.path.join(scitos_dir, 'station', 'station.pcd') + default_dock_database = os.path.join(scitos_dir, 'params', 'docks.yaml') + + # Create the launch configuration variables. + namespace = LaunchConfiguration('namespace') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['docking_server'] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'autostart': autostart, + 'dock_template': default_template_file, + 'dock_database': default_dock_database, + } + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True + ) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) + + # Map these variables to arguments: can be set from the command line or a default will be used + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace' + ) + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=default_params_file, + description='Full path to the Mira parameter file to use' + ) + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='False', + description='Use composed bringup if True', + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', + default_value='info', + description='log level' + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='opennav_docking', + executable='opennav_docking', + name='docking_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', ['docking_server:=', log_level]], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_scitos', + output='screen', + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ], + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + LoadComposableNodes( + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='opennav_docking', + plugin='opennav_docking::DockingServer', + name='docking_server', + parameters=[configured_params], + ), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_scitos', + parameters=[ + {'autostart': autostart, 'node_names': lifecycle_nodes} + ], + ), + ], + ), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/scitos2_charging_dock/package.xml b/scitos2_charging_dock/package.xml new file mode 100644 index 0000000..d54dc85 --- /dev/null +++ b/scitos2_charging_dock/package.xml @@ -0,0 +1,32 @@ + + + + scitos2_charging_dock + 3.0.0 + The scitos2_charging_docks package + Alberto Tudela + Apache-2.0 + Alberto Tudela + ament_cmake + rclcpp + rclcpp_components + pluginlib + geometry_msgs + sensor_msgs + nav2_util + pcl_ros + pcl_conversions + tf2_ros + scitos2_msgs + opennav_docking_core + opennav_docking + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + ament_index_cpp + + + ament_cmake + + diff --git a/scitos2_charging_dock/params/docks.yaml b/scitos2_charging_dock/params/docks.yaml new file mode 100644 index 0000000..6d6fb6a --- /dev/null +++ b/scitos2_charging_dock/params/docks.yaml @@ -0,0 +1,9 @@ +# ---------------- +# Docks in the map +# ---------------- + +docks: + main_dock: + type: "scitos_dock" + frame: "map" + pose: [0.0, 0.0, 0.0] \ No newline at end of file diff --git a/scitos2_charging_dock/params/example.yaml b/scitos2_charging_dock/params/example.yaml new file mode 100644 index 0000000..8fb70a7 --- /dev/null +++ b/scitos2_charging_dock/params/example.yaml @@ -0,0 +1,61 @@ +#--------------- +# Docking server +#--------------- +# See param description on https://docs.nav2.org/tutorials/docs/using_docking.html#docking-tutorial + +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 15.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Docking controller parameters + controller: + k_phi: 1.0 + k_delta: 3.0 + beta: 0.4 + lambda: 2.0 + v_linear_min: 0.1 + v_linear_max: 0.2 + max_angular_velocity: 0.5 + + # Types of docks + dock_plugins: ["scitos_dock"] + dock_database: '' + scitos_dock: + plugin: "scitos2_charging_dock::ChargingDock" + docking_threshold: 0.05 + staging_x_offset: -0.75 + staging_yaw_offset: 0.0 + external_detection_timeout: 5.0 + external_detection_translation_x: -0.20 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: 0.0 + external_detection_rotation_pitch: 0.0 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + perception: + icp_min_score: 0.01 + icp_max_iter: 300 + icp_max_corr_dis: 0.25 + icp_max_trans_eps: 1.0e-8 + icp_max_eucl_fit_eps: 1.0e-8 + enable_debug: true + dock_template: '' + segmentation: + distance_threshold: 0.04 + min_points_segment: 20 + max_points_segment: 400 + min_distance: 0.0 + max_distance: 2.0 + min_segment_width: 0.3 + max_segment_width: 1.0 + diff --git a/scitos2_charging_dock/plugins.xml b/scitos2_charging_dock/plugins.xml new file mode 100644 index 0000000..9952f63 --- /dev/null +++ b/scitos2_charging_dock/plugins.xml @@ -0,0 +1,7 @@ + + + + A Scitos charging dock plugin. + + + diff --git a/scitos2_charging_dock/src/charging_dock.cpp b/scitos2_charging_dock/src/charging_dock.cpp new file mode 100644 index 0000000..41ea3e4 --- /dev/null +++ b/scitos2_charging_dock/src/charging_dock.cpp @@ -0,0 +1,235 @@ +// Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "nav2_util/node_utils.hpp" +#include "scitos2_charging_dock/charging_dock.hpp" +#include "scitos2_charging_dock/segmentation.hpp" + +namespace scitos2_charging_dock +{ + +void ChargingDock::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf) +{ + name_ = name; + tf2_buffer_ = tf; + is_charging_ = false; + node_ = parent.lock(); + if (!node_) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Parameters for optional external detection of dock pose + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_timeout", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_translation_x", rclcpp::ParameterValue(-0.20)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_translation_y", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_yaw", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_pitch", rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_roll", rclcpp::ParameterValue(-1.57)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".filter_coef", rclcpp::ParameterValue(0.1)); + + // This is how close robot should get to pose + nav2_util::declare_parameter_if_not_declared( + node_, name + ".docking_threshold", rclcpp::ParameterValue(0.05)); + + // Staging pose configuration + nav2_util::declare_parameter_if_not_declared( + node_, name + ".staging_x_offset", rclcpp::ParameterValue(-0.7)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); + + node_->get_parameter(name + ".external_detection_timeout", external_detection_timeout_); + node_->get_parameter( + name + ".external_detection_translation_x", external_detection_translation_x_); + node_->get_parameter( + name + ".external_detection_translation_y", external_detection_translation_y_); + double yaw, pitch, roll; + node_->get_parameter(name + ".external_detection_rotation_yaw", yaw); + node_->get_parameter(name + ".external_detection_rotation_pitch", pitch); + node_->get_parameter(name + ".external_detection_rotation_roll", roll); + external_detection_rotation_.setEuler(pitch, roll, yaw); + node_->get_parameter(name + ".docking_threshold", docking_threshold_); + node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); + node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + + // Setup perception + perception_ = std::make_unique(node_, name, tf2_buffer_); + + // Setup filter + double filter_coef; + node_->get_parameter(name + ".filter_coef", filter_coef); + filter_ = std::make_unique(filter_coef, external_detection_timeout_); + + battery_sub_ = node_->create_subscription( + "battery", 1, + [this]( + const sensor_msgs::msg::BatteryState::SharedPtr state) { + is_charging_ = + (state->power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING); + }); + + dock_pose_.header.stamp = rclcpp::Time(0); + scan_sub_ = node_->create_subscription( + "scan", rclcpp::SensorDataQoS(), + [this](const sensor_msgs::msg::LaserScan::SharedPtr scan) { + scan_ = *scan; + }); + + dock_pose_pub_ = node_->create_publisher("dock_pose", 1); + filtered_dock_pose_pub_ = node_->create_publisher( + "filtered_dock_pose", 1); + staging_pose_pub_ = node_->create_publisher("staging_pose", 1); +} + +geometry_msgs::msg::PoseStamped ChargingDock::getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame) +{ + // Send the initial estimate to the perception module + perception_->setInitialEstimate(pose, frame); + + // Compute the staging pose with given offsets + const double yaw = tf2::getYaw(pose.orientation); + geometry_msgs::msg::PoseStamped staging_pose; + staging_pose.header.frame_id = frame; + staging_pose.header.stamp = node_->now(); + staging_pose.pose = pose; + staging_pose.pose.position.x += cos(yaw) * staging_x_offset_; + staging_pose.pose.position.y += sin(yaw) * staging_x_offset_; + tf2::Quaternion orientation; + orientation.setEuler(0.0, 0.0, yaw + staging_yaw_offset_); + staging_pose.pose.orientation = tf2::toMsg(orientation); + + // Publish staging pose for debugging purposes + staging_pose_pub_->publish(staging_pose); + return staging_pose; +} + +bool ChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose) +{ + // Get current detections, transform to frame, and apply offsets + geometry_msgs::msg::PoseStamped detected = perception_->getDockPose(scan_); + + // Validate that external pose is new enough + auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_); + if (node_->now() - detected.header.stamp > timeout) { + RCLCPP_WARN(node_->get_logger(), "Lost detection or did not detect: timeout exceeded"); + return false; + } + + // Transform detected pose into fixed frame. Note that the argument pose + // is the output of detection, but also acts as the initial estimate + // and contains the frame_id of docking + if (detected.header.frame_id != pose.header.frame_id) { + try { + if (!tf2_buffer_->canTransform( + pose.header.frame_id, detected.header.frame_id, + detected.header.stamp, rclcpp::Duration::from_seconds(0.2))) + { + RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); + return false; + } + tf2_buffer_->transform(detected, detected, pose.header.frame_id); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); + return false; + } + } + + // Filter the detected pose + detected = filter_->update(detected); + filtered_dock_pose_pub_->publish(detected); + + // Rotate the just the orientation, then remove roll/pitch + geometry_msgs::msg::PoseStamped just_orientation; + just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_); + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation = detected.pose.orientation; + tf2::doTransform(just_orientation, just_orientation, transform); + + tf2::Quaternion orientation; + orientation.setEuler(0.0, 0.0, tf2::getYaw(just_orientation.pose.orientation)); + dock_pose_.pose.orientation = tf2::toMsg(orientation); + + // Construct dock_pose_ by applying translation/rotation + dock_pose_.header = detected.header; + dock_pose_.pose.position = detected.pose.position; + const double yaw = tf2::getYaw(dock_pose_.pose.orientation); + dock_pose_.pose.position.x += cos(yaw) * external_detection_translation_x_ - + sin(yaw) * external_detection_translation_y_; + dock_pose_.pose.position.y += sin(yaw) * external_detection_translation_x_ + + cos(yaw) * external_detection_translation_y_; + dock_pose_.pose.position.z = 0.0; + + // Publish & return dock pose for debugging purposes + dock_pose_pub_->publish(dock_pose_); + pose = dock_pose_; + return true; +} + +bool ChargingDock::isCharging() +{ + return is_charging_; +} + +bool ChargingDock::isDocked() +{ + if (dock_pose_.header.frame_id.empty()) { + // Dock pose is not yet valid + return false; + } + + // Find base pose in target frame + geometry_msgs::msg::PoseStamped base_pose; + base_pose.header.stamp = rclcpp::Time(0); + base_pose.header.frame_id = "base_link"; + base_pose.pose.orientation.w = 1.0; + try { + tf2_buffer_->transform(base_pose, base_pose, dock_pose_.header.frame_id); + } catch (const tf2::TransformException & ex) { + return false; + } + + // If we are close enough, pretend we are charging + double d = std::hypot( + base_pose.pose.position.x - dock_pose_.pose.position.x, + base_pose.pose.position.y - dock_pose_.pose.position.y); + return d < docking_threshold_; +} + +bool ChargingDock::disableCharging() +{ + return true; +} + +bool ChargingDock::hasStoppedCharging() +{ + return !isCharging(); +} + +} // namespace scitos2_charging_dock + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(scitos2_charging_dock::ChargingDock, opennav_docking_core::ChargingDock) diff --git a/scitos2_charging_dock/src/dock_saver.cpp b/scitos2_charging_dock/src/dock_saver.cpp new file mode 100644 index 0000000..68d3c4d --- /dev/null +++ b/scitos2_charging_dock/src/dock_saver.cpp @@ -0,0 +1,184 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "sensor_msgs/msg/laser_scan.hpp" +#include "scitos2_charging_dock/dock_saver.hpp" + +namespace scitos2_charging_dock +{ +DockSaver::DockSaver(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("dock_saver", "", options) +{ + RCLCPP_INFO(get_logger(), "Creating the dock saver node"); + + // Declare the node parameters + declare_parameter("save_dock_timeout", 2.0); +} + +DockSaver::~DockSaver() +{ +} + +nav2_util::CallbackReturn +DockSaver::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Make name prefix for services + const std::string service_prefix = get_name() + std::string("/"); + + save_dock_timeout_ = std::make_shared( + rclcpp::Duration::from_seconds(get_parameter("save_dock_timeout").as_double())); + + // Create a service that saves the pointcloud from a dock to a file + save_dock_service_ = create_service( + service_prefix + save_dock_service_name_, + std::bind(&DockSaver::saveDockCallback, this, std::placeholders::_1, std::placeholders::_2)); + + // Setup TF buffer and perception + tf2_buffer_ = std::make_shared(get_clock()); + perception_ = std::make_unique(shared_from_this(), "dock_saver", tf2_buffer_); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +DockSaver::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + // create bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +DockSaver::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + // destroy bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +DockSaver::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + save_dock_service_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +DockSaver::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} + +bool DockSaver::saveDockCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO( + get_logger(), "Saving dock pointcloud from %s topic to file: %s", request->scan_topic.c_str(), + request->dock_url.c_str()); + + // Correct scan_topic if necessary + std::string scan_topic = request->scan_topic; + if (scan_topic == "") { + scan_topic = "scan"; + RCLCPP_WARN( + get_logger(), "Scan topic unspecified. Using default scan topic: %s.", scan_topic.c_str()); + } + + // Checking dock file name + std::string filename = request->dock_url; + if (filename == "") { + filename = "dock_" + std::to_string(static_cast(get_clock()->now().seconds())); + RCLCPP_WARN( + get_logger(), "Dock file unspecified. Dock will be saved to %s file", filename.c_str()); + } + + // Create a callback function that receives the scan from subscribed topic + std::promise prom; + std::future future_result = prom.get_future(); + auto scanCallback = [&prom]( + const sensor_msgs::msg::LaserScan::SharedPtr msg) -> void { + prom.set_value(msg); + }; + + // Create new CallbackGroup for scan subscription + auto callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto option = rclcpp::SubscriptionOptions(); + option.callback_group = callback_group; + + auto scan_sub = create_subscription( + scan_topic, rclcpp::SensorDataQoS(), scanCallback, option); + + // Create SingleThreadedExecutor to spin scan_sub in callback_group + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_callback_group(callback_group, get_node_base_interface()); + // Spin until dock message received + auto timeout = save_dock_timeout_->to_chrono(); + auto status = executor.spin_until_future_complete(future_result, timeout); + if (status != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Failed to spin scan subscription"); + return false; + } + // scan_sub is no more needed + scan_sub.reset(); + + // Scan message received. Extracting dock pointcloud + sensor_msgs::msg::LaserScan::SharedPtr scan_msg = future_result.get(); + + // Extract clusters from the scan + auto clusters = perception_->extractClustersFromScan(*scan_msg); + + if (clusters.empty()) { + RCLCPP_ERROR(get_logger(), "No clusters found in the scan"); + return false; + } + + // Find closest cluster + std::vector distances; + for (auto cluster : clusters) { + double distance = std::hypot(cluster.getCentroid().x, cluster.getCentroid().y); + distances.push_back(distance); + } + auto min_distance = std::min_element(distances.begin(), distances.end()); + int idx = std::distance(distances.begin(), min_distance); + + // Store the dock pointcloud to a file + scitos2_charging_dock::Pcloud::Ptr dock = clusters[idx].cloud; + response->result = perception_->storeDockPointcloud(filename, *dock); + + return true; +} +} // namespace scitos2_charging_dock + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(scitos2_charging_dock::DockSaver) diff --git a/scitos2_charging_dock/src/main_saver.cpp b/scitos2_charging_dock/src/main_saver.cpp new file mode 100644 index 0000000..9b1524c --- /dev/null +++ b/scitos2_charging_dock/src/main_saver.cpp @@ -0,0 +1,28 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/rclcpp.hpp" +#include "scitos2_charging_dock/dock_saver.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; +} diff --git a/scitos2_charging_dock/src/perception.cpp b/scitos2_charging_dock/src/perception.cpp new file mode 100644 index 0000000..f232be4 --- /dev/null +++ b/scitos2_charging_dock/src/perception.cpp @@ -0,0 +1,384 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// PCL +#include +#include +#include + +// TF +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +// ROS +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav2_util/node_utils.hpp" +#include "pcl_conversions/pcl_conversions.h" +#include "pcl_ros/transforms.hpp" +#include "scitos2_charging_dock/perception.hpp" + +namespace scitos2_charging_dock +{ + +Perception::Perception( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & name, std::shared_ptr tf) +{ + name_ = name; + logger_ = node->get_logger(); + clock_ = node->get_clock(); + tf_buffer_ = tf; + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".perception.icp_min_score", rclcpp::ParameterValue(0.01)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".perception.icp_max_iter", rclcpp::ParameterValue(300)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".perception.icp_max_corr_dis", rclcpp::ParameterValue(0.25)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".perception.icp_max_trans_eps", rclcpp::ParameterValue(0.000000001)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".perception.icp_max_eucl_fit_eps", rclcpp::ParameterValue(0.000000001)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".perception.enable_debug", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".perception.dock_template", rclcpp::ParameterValue("")); + + node->get_parameter(name_ + ".perception.icp_min_score", icp_min_score_); + node->get_parameter(name_ + ".perception.icp_max_iter", icp_max_iter_); + node->get_parameter(name_ + ".perception.icp_max_corr_dis", icp_max_corr_dis_); + node->get_parameter(name_ + ".perception.icp_max_trans_eps", icp_max_trans_eps_); + node->get_parameter(name_ + ".perception.icp_max_eucl_fit_eps", icp_max_eucl_fit_eps_); + node->get_parameter(name_ + ".perception.enable_debug", debug_); + + // Load the dock template + std::string dock_template; + dock_template_ = Pcloud::Ptr(new Pcloud); + node->get_parameter(name_ + ".perception.dock_template", dock_template); + loadDockPointcloud(dock_template, *dock_template_); + + // Publishers + if (debug_) { + target_cloud_pub_ = node->create_publisher("dock/target", 1); + dock_cloud_pub_ = node->create_publisher("dock/cloud", 1); + dock_template_pub_ = node->create_publisher( + "dock/template", 1); + } + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Perception::dynamicParametersCallback, this, std::placeholders::_1)); + + // Setup the segmentation object + segmentation_ = std::make_unique(node, name); + + // Set the verbosity level of the PCL library to shutout the console output + pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); +} + +Perception::~Perception() +{ + segmentation_.reset(); + tf_buffer_.reset(); +} + +geometry_msgs::msg::PoseStamped Perception::getDockPose(const sensor_msgs::msg::LaserScan & scan) +{ + // Extract clusters from the scan + auto clusters = extractClustersFromScan(scan); + + // Set the header of the template to the scan header + dock_template_->header.frame_id = scan.header.frame_id; + + // Refine the pose of each cluster to get the dock pose + geometry_msgs::msg::PoseStamped dock_pose; + if (refineAllClustersPoses(clusters, dock_template_, dock_pose)) { + detected_dock_pose_ = dock_pose; + } + return detected_dock_pose_; +} + +void Perception::setInitialEstimate( + const geometry_msgs::msg::Pose & pose, const std::string & frame) +{ + initial_estimate_pose_.pose = pose; + initial_estimate_pose_.header.frame_id = frame; + initial_estimate_pose_.header.stamp = clock_->now(); +} + +bool Perception::loadDockPointcloud(std::string filepath, Pcloud & dock) +{ + if (filepath.empty()) { + RCLCPP_ERROR(logger_, "Couldn't load the dock from an empty file path"); + } else { + if (pcl::io::loadPCDFile(filepath, dock) < 0) { + RCLCPP_ERROR(logger_, "Failed to load the dock from PCD file"); + } else { + RCLCPP_INFO(logger_, "Dock loaded from PCD file with %lu points", dock.size()); + return true; + } + } + return false; +} + +bool Perception::storeDockPointcloud(std::string filepath, const Pcloud & dock) +{ + bool success = false; + if (pcl::io::savePCDFile(filepath, dock) < 0) { + RCLCPP_ERROR(logger_, "Failed to save the dock to PCD file"); + } else { + RCLCPP_INFO(logger_, "Dock saved to PCD file"); + success = true; + } + return success; +} + +Clusters Perception::extractClustersFromScan(const sensor_msgs::msg::LaserScan & scan) +{ + // Perform segmentation on the scan and filter the segments + Segments segments; + if (segmentation_->performSegmentation(scan, segments)) { + auto filtered_segments = segmentation_->filterSegments(segments); + // Convert the segments to clusters + return segmentsToClusters(scan.header.frame_id, filtered_segments); + } else { + return Clusters(); + } +} + +Clusters Perception::segmentsToClusters(std::string frame, const Segments & segments) +{ + Clusters clusters; + int count = 0; + for (const auto & segment : segments) { + Cluster cluster; + cluster.id = count++; + cluster.matched_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); + cluster.cloud = segmentToPcloud(frame, segment); + clusters.push_back(cluster); + } + return clusters; +} + +Pcloud::Ptr Perception::segmentToPcloud(std::string frame, const Segment & segment) +{ + Pcloud::Ptr cloud(new pcl::PointCloud); + cloud->header.frame_id = frame; + + for (const auto & point : segment.points) { + pcl::PointXYZ pcl_point; + pcl_point.x = point.x; + pcl_point.y = point.y; + pcl_point.z = point.z; + cloud->push_back(pcl_point); + } + + return cloud; +} + +bool Perception::refineClusterPose(Cluster & cluster, Pcloud::ConstPtr cloud_template) +{ + bool success = false; + + // Prepare the ICP object + pcl::IterativeClosestPoint icp; + icp.setMaximumIterations(icp_max_iter_); + icp.setMaxCorrespondenceDistance(icp_max_corr_dis_); + icp.setTransformationEpsilon(icp_max_trans_eps_); + icp.setEuclideanFitnessEpsilon(icp_max_eucl_fit_eps_); + + // Align the cluster to the template + icp.setInputSource(cloud_template); + icp.setInputTarget(cluster.cloud); + Pcloud::Ptr matched_cloud(new pcl::PointCloud()); + icp.align(*matched_cloud); + + // If the ICP converged, store the results on the cluster + if (icp.hasConverged()) { + // Get the final transformation + auto icp_refinement = eigenToTransform(icp.getFinalTransformation()); + // Transform the pose to the matching frame + tf2::Transform tf_stage; + tf2::fromMsg(initial_estimate_pose_.pose, tf_stage); + tf2::Transform tf_correct = icp_refinement * tf_stage; + // Convert to ROS msg + geometry_msgs::msg::Pose pose_correct; + tf2::toMsg(tf_correct, pose_correct); + + // Update the cluster + cluster.icp_score = icp.getFitnessScore(); + cluster.icp_pose.header.frame_id = initial_estimate_pose_.header.frame_id; + cluster.icp_pose.header.stamp = clock_->now(); + cluster.icp_pose.pose = pose_correct; + cluster.matched_cloud = matched_cloud; + + success = true; + + RCLCPP_DEBUG(logger_, "ICP converged for cluster %i", cluster.id); + RCLCPP_DEBUG(logger_, "ICP fitness score: %f", cluster.icp_score); + RCLCPP_DEBUG( + logger_, "ICP transformation: (%f, %f)", + cluster.icp_pose.pose.position.x, cluster.icp_pose.pose.position.y); + } else { + RCLCPP_DEBUG(logger_, "ICP did not converge for cluster %i", cluster.id); + } + + return success; +} + +bool Perception::refineAllClustersPoses( + Clusters & clusters, Pcloud::ConstPtr cloud_template, geometry_msgs::msg::PoseStamped & dock_pose) +{ + bool success = false; + Clusters potential_docks; + + for (auto & cluster : clusters) { + // Discard clusters not valid (i.e. clusters not similar to dock by size, ...) + double dx = (*cloud_template).back().x - (*cloud_template).front().x; + double dy = (*cloud_template).back().y - (*cloud_template).front().y; + double template_width = std::hypot(dx, dy); + if (!cluster.valid(template_width)) { + continue; + } + + // Prepares the template pointcloud for matching by transforming to the initial estimate pose. + // Usually in global coordinates (map frame) + Pcloud::Ptr cloud_template_initial(new pcl::PointCloud); + tf2::Transform tf_stage; + tf2::fromMsg(initial_estimate_pose_.pose, tf_stage); + pcl_ros::transformPointCloud(*cloud_template, *cloud_template_initial, tf_stage); + + // Transforms the target point cloud from the scan frame to the global frame (map frame) + if (cluster.cloud->header.frame_id != initial_estimate_pose_.header.frame_id) { + try { + if (!tf_buffer_->canTransform( + initial_estimate_pose_.header.frame_id, cluster.cloud->header.frame_id, + rclcpp::Time(cluster.cloud->header.stamp * 1000), rclcpp::Duration::from_seconds(0.2))) + { + RCLCPP_WARN( + logger_, "Could not transform %s to %s", + cluster.cloud->header.frame_id.c_str(), initial_estimate_pose_.header.frame_id.c_str()); + return false; + } + auto tf_stamped = tf_buffer_->lookupTransform( + initial_estimate_pose_.header.frame_id, cluster.cloud->header.frame_id, + tf2::TimePointZero); + pcl_ros::transformPointCloud(*(cluster.cloud), *(cluster.cloud), tf_stamped); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN( + logger_, "Could not transform %s to %s: %s", + cluster.cloud->header.frame_id.c_str(), + initial_estimate_pose_.header.frame_id.c_str(), ex.what()); + return false; + } + } + + // Visualizes the target point cloud and estimated dock template pose + if (debug_) { + // Publish dock template + sensor_msgs::msg::PointCloud2 template_msg; + pcl::toROSMsg(*cloud_template, template_msg); + template_msg.header.frame_id = initial_estimate_pose_.header.frame_id; + template_msg.header.stamp = clock_->now(); + dock_template_pub_->publish(std::move(template_msg)); + + // Publish cluster target + sensor_msgs::msg::PointCloud2 target_msg; + pcl::toROSMsg(*cluster.cloud, target_msg); + target_msg.header.frame_id = initial_estimate_pose_.header.frame_id; + target_msg.header.stamp = clock_->now(); + target_cloud_pub_->publish(std::move(target_msg)); + } + + // Refine the cluster to get the dock pose + if (refineClusterPose(cluster, cloud_template_initial)) { + // Check if potential dock is found + if (cluster.icp_score < icp_min_score_) { + RCLCPP_DEBUG( + logger_, "Dock potentially identified at cluster %i with score %f", + cluster.id, cluster.icp_score); + potential_docks.push_back(cluster); + } + } + } + + // Check if dock is found + if (!potential_docks.empty()) { + // Sort the clusters by ICP score + std::sort(potential_docks.begin(), potential_docks.end()); + dock_pose = potential_docks.front().icp_pose; + // Publish the dock cloud + if (debug_) { + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(*(clusters.front().matched_cloud), cloud_msg); + cloud_msg.header.frame_id = initial_estimate_pose_.header.frame_id; + cloud_msg.header.stamp = clock_->now(); + dock_cloud_pub_->publish(std::move(cloud_msg)); + } + success = true; + RCLCPP_DEBUG(logger_, "Dock successfully identified at cluster %i", potential_docks.front().id); + } else { + RCLCPP_WARN(logger_, "Unable to identify the dock"); + } + + return success; +} + +tf2::Transform Perception::eigenToTransform(const Eigen::Matrix4f & T) +{ + return tf2::Transform( + tf2::Matrix3x3( + T(0, 0), T(0, 1), T(0, 2), + T(1, 0), T(1, 1), T(1, 2), + T(2, 0), T(2, 1), T(2, 2)), + tf2::Vector3(T(0, 3), T(1, 3), T(2, 3))); +} + +rcl_interfaces::msg::SetParametersResult Perception::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(dynamic_params_lock_); + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == rclcpp::ParameterType::PARAMETER_INTEGER) { + if (name == name_ + ".perception.icp_max_iter") { + icp_max_iter_ = parameter.as_int(); + } + } else if (type == rclcpp::ParameterType::PARAMETER_DOUBLE) { + if (name == name_ + ".perception.icp_min_score") { + icp_min_score_ = parameter.as_double(); + } else if (name == name_ + ".perception.icp_max_corr_dis") { + icp_max_corr_dis_ = parameter.as_double(); + } else if (name == name_ + ".perception.icp_max_trans_eps") { + icp_max_trans_eps_ = parameter.as_double(); + } else if (name == name_ + ".perception.icp_max_eucl_fit_eps") { + icp_max_eucl_fit_eps_ = parameter.as_double(); + } + } else if (type == rclcpp::ParameterType::PARAMETER_BOOL) { + if (name == name_ + ".perception.enable_debug") { + debug_ = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace scitos2_charging_dock diff --git a/scitos2_charging_dock/src/segmentation.cpp b/scitos2_charging_dock/src/segmentation.cpp new file mode 100644 index 0000000..3601d42 --- /dev/null +++ b/scitos2_charging_dock/src/segmentation.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2017 Alberto J. Tudela Roldán +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/node_utils.hpp" +#include "scitos2_charging_dock/segmentation.hpp" + +namespace scitos2_charging_dock +{ + +Segmentation::Segmentation( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & name) +{ + name_ = name; + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".segmentation.distance_threshold", rclcpp::ParameterValue(0.04)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".segmentation.min_points", rclcpp::ParameterValue(25)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".segmentation.max_points", rclcpp::ParameterValue(400)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".segmentation.min_distance", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".segmentation.max_distance", rclcpp::ParameterValue(2.0)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".segmentation.min_width", rclcpp::ParameterValue(0.3)); + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".segmentation.max_width", rclcpp::ParameterValue(1.0)); + + node->get_parameter(name_ + ".segmentation.distance_threshold", distance_threshold_); + node->get_parameter(name_ + ".segmentation.min_points", min_points_segment_); + node->get_parameter(name_ + ".segmentation.max_points", max_points_segment_); + node->get_parameter(name_ + ".segmentation.min_distance", min_avg_distance_from_sensor_); + node->get_parameter(name_ + ".segmentation.max_distance", max_avg_distance_from_sensor_); + node->get_parameter(name_ + ".segmentation.min_width", min_segment_width_); + node->get_parameter(name_ + ".segmentation.max_width", max_segment_width_); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Segmentation::dynamicParametersCallback, this, std::placeholders::_1)); +} + +/* This code is a simplification of the segmentation from package laser_segmentation + * by Alberto Tudela. + */ +bool Segmentation::performSegmentation( + const sensor_msgs::msg::LaserScan & scan, Segments & segments) +{ + // Convert the scan to points + auto points = scanToPoints(scan); + + if (points.empty()) { + return false; + } + + // Create the first segment + Segment current_segment; + current_segment.points.push_back(points.front()); + + // Create the segments + for (uint64_t p = 1; p < points.size(); p++) { + if (isJumpBetweenPoints(points[p - 1], points[p], distance_threshold_)) { + segments.push_back(current_segment); + current_segment.points.clear(); + } + current_segment.points.push_back(points[p]); + } + segments.push_back(current_segment); + + return segments.size() > 0; +} + +Segments Segmentation::filterSegments(const Segments & segments) +{ + Segments filtered_segments; + filtered_segments.reserve(segments.size()); + + const double squared_min_segment_width = min_segment_width_ * min_segment_width_; + const double squared_max_segment_width = max_segment_width_ * max_segment_width_; + + for (const auto & segment : segments) { + // By number of points + if (segment.size() < min_points_segment_ || segment.size() > max_points_segment_) { + continue; + } + + // By distance to sensor + if (segment.centroid_length() < min_avg_distance_from_sensor_ || + segment.centroid_length() > max_avg_distance_from_sensor_) + { + continue; + } + + // By width + if (segment.width_squared() < squared_min_segment_width || + segment.width_squared() > squared_max_segment_width) + { + continue; + } + + filtered_segments.push_back(segment); + } + return filtered_segments; +} + +geometry_msgs::msg::Point Segmentation::fromPolarToCartesian(double range, double angle) +{ + geometry_msgs::msg::Point point; + point.x = range * cos(angle); + point.y = range * sin(angle); + return point; +} + +std::vector Segmentation::scanToPoints( + const sensor_msgs::msg::LaserScan & scan) +{ + std::vector points; + double phi = scan.angle_min; + double angle_resolution = scan.angle_increment; + for (const auto r : scan.ranges) { + if (r >= scan.range_min && r <= scan.range_max) { + points.push_back(fromPolarToCartesian(r, phi)); + } + phi += angle_resolution; + } + return points; +} + +double Segmentation::euclideanDistance( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2)); +} + +bool Segmentation::isJumpBetweenPoints( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, double threshold) +{ + return euclideanDistance(p1, p2) > threshold; +} + +rcl_interfaces::msg::SetParametersResult +Segmentation::dynamicParametersCallback(std::vector parameters) +{ + std::lock_guard lock(dynamic_params_lock_); + + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + if (type == rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) { + if (name == name_ + ".segmentation.min_points") { + min_points_segment_ = parameter.as_int(); + } else if (name == name_ + ".segmentation.max_points") { + max_points_segment_ = parameter.as_int(); + } + } else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { + if (name == name_ + ".segmentation.distance_threshold") { + distance_threshold_ = parameter.as_double(); + } else if (name == name_ + ".segmentation.min_distance") { + min_avg_distance_from_sensor_ = parameter.as_double(); + } else if (name == name_ + ".segmentation.max_distance") { + max_avg_distance_from_sensor_ = parameter.as_double(); + } else if (name == name_ + ".segmentation.min_width") { + min_segment_width_ = parameter.as_double(); + } else if (name == name_ + ".segmentation.max_width") { + max_segment_width_ = parameter.as_double(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace scitos2_charging_dock diff --git a/scitos2_charging_dock/test/CMakeLists.txt b/scitos2_charging_dock/test/CMakeLists.txt new file mode 100644 index 0000000..226e173 --- /dev/null +++ b/scitos2_charging_dock/test/CMakeLists.txt @@ -0,0 +1,54 @@ +# Test cluster +ament_add_gtest(test_scitos2_cluster + test_cluster.cpp +) +ament_target_dependencies(test_scitos2_cluster + ${dependencies} +) +target_link_libraries(test_scitos2_cluster + ${library_name} +) + +# Test segmentation +ament_add_gtest(test_scitos2_segmentation + test_segmentation.cpp +) +ament_target_dependencies(test_scitos2_segmentation + ${dependencies} +) +target_link_libraries(test_scitos2_segmentation + ${library_name} +) + +# Test perception +ament_add_gtest(test_scitos2_perception + test_perception.cpp +) +ament_target_dependencies(test_scitos2_perception + ${dependencies} +) +target_link_libraries(test_scitos2_perception + ${library_name} +) + +# Test charging dock +ament_add_gtest(test_scitos2_charging_dock + test_charging_dock.cpp +) +ament_target_dependencies(test_scitos2_charging_dock + ${dependencies} +) +target_link_libraries(test_scitos2_charging_dock + ${library_name} +) + +# Test dock saver +ament_add_gtest(test_dock_saver + test_dock_saver.cpp +) +ament_target_dependencies(test_dock_saver + ${dependencies} +) +target_link_libraries(test_dock_saver + ${library_name} +) \ No newline at end of file diff --git a/scitos2_charging_dock/test/dock_test.pcd b/scitos2_charging_dock/test/dock_test.pcd new file mode 100644 index 0000000..8104c94 --- /dev/null +++ b/scitos2_charging_dock/test/dock_test.pcd @@ -0,0 +1,14 @@ +# .PCD v0.7 - Point Cloud Data file format +VERSION 0.7 +FIELDS x y z +SIZE 4 4 4 +TYPE F F F +COUNT 1 1 1 +WIDTH 3 +HEIGHT 1 +VIEWPOINT 0 0 0 1 0 0 0 +POINTS 3 +DATA ascii +0.9 -0.1 0.0 +1.0 0.0 0.0 +0.9 0.1 0.0 \ No newline at end of file diff --git a/scitos2_charging_dock/test/empty_dock_test.pcd b/scitos2_charging_dock/test/empty_dock_test.pcd new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/scitos2_charging_dock/test/empty_dock_test.pcd @@ -0,0 +1 @@ + diff --git a/scitos2_charging_dock/test/test_charging_dock.cpp b/scitos2_charging_dock/test/test_charging_dock.cpp new file mode 100644 index 0000000..bc66c4f --- /dev/null +++ b/scitos2_charging_dock/test/test_charging_dock.cpp @@ -0,0 +1,208 @@ +// Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "nav2_util/node_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" +#include "scitos2_charging_dock/charging_dock.hpp" + +TEST(ScitosChargingDock, objectLifecycle) +{ + auto node = std::make_shared("test"); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // Check initial states + EXPECT_FALSE(dock->isCharging()); + EXPECT_TRUE(dock->disableCharging()); + EXPECT_TRUE(dock->hasStoppedCharging()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(ScitosChargingDock, batteryState) +{ + auto node = std::make_shared("test"); + auto pub = node->create_publisher( + "battery", rclcpp::QoS(1)); + pub->on_activate(); + + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // Not charging + sensor_msgs::msg::BatteryState msg; + msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING; + pub->publish(msg); + rclcpp::Rate r(2); + r.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_FALSE(dock->isCharging()); + EXPECT_TRUE(dock->hasStoppedCharging()); + + // Charging + sensor_msgs::msg::BatteryState msg2; + msg2.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING; + pub->publish(msg2); + rclcpp::Rate r1(2); + r1.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_TRUE(dock->isCharging()); + EXPECT_FALSE(dock->hasStoppedCharging()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(ScitosChargingDock, stagingPose) +{ + auto node = std::make_shared("test"); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::Pose pose; + std::string frame = "my_frame"; + auto staging_pose = dock->getStagingPose(pose, frame); + EXPECT_NEAR(staging_pose.pose.position.x, -0.7, 0.01); + EXPECT_NEAR(staging_pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(staging_pose.pose.orientation), 0.0, 0.01); + EXPECT_EQ(staging_pose.header.frame_id, frame); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(ScitosChargingDock, stagingPoseWithYawOffset) +{ + // Override the parameter default + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.staging_yaw_offset", 3.14}, + } + ); + + auto node = std::make_shared("test", options); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::Pose pose; + std::string frame = "my_frame"; + auto staging_pose = dock->getStagingPose(pose, frame); + // Pose should be the same as default, but pointing in opposite direction + EXPECT_NEAR(staging_pose.pose.position.x, -0.7, 0.01); + EXPECT_NEAR(staging_pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(staging_pose.pose.orientation), 3.14, 0.01); + EXPECT_EQ(staging_pose.header.frame_id, frame); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(ScitosChargingDock, refinedPoseTest) +{ + auto node = std::make_shared("test"); + auto pub = node->create_publisher("scan", 1); + pub->on_activate(); + auto dock = std::make_unique(); + + // Create the TF + auto tf_buffer = std::make_shared(node->get_clock()); + + // Update parameters to read the test dock template + std::string pkg = ament_index_cpp::get_package_share_directory("scitos2_charging_dock"); + std::string path = pkg + "/test/dock_test.pcd"; + nav2_util::declare_parameter_if_not_declared( + node, "my_dock.perception.dock_template", rclcpp::ParameterValue(path)); + nav2_util::declare_parameter_if_not_declared( + node, "my_dock.segmentation.distance_threshold", rclcpp::ParameterValue(0.5)); + nav2_util::declare_parameter_if_not_declared( + node, "my_dock.segmentation.min_points", rclcpp::ParameterValue(0)); + nav2_util::declare_parameter_if_not_declared( + node, "my_dock.segmentation.min_width", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node, "my_dock.segmentation.min_distance", rclcpp::ParameterValue(0.0)); + + dock->configure(node, "my_dock", tf_buffer); + dock->activate(); + + geometry_msgs::msg::PoseStamped pose; + + // Timestamps are outdated; this is after timeout + EXPECT_FALSE(dock->isDocked()); + EXPECT_FALSE(dock->getRefinedPose(pose)); + + // Just call the function to set the staging pose in the perception module + // befor the callback is called + geometry_msgs::msg::Pose pose_stmp; + std::string frame = "my_frame"; + auto staging_pose = dock->getStagingPose(pose_stmp, frame); + + // Publish a scan + sensor_msgs::msg::LaserScan scan; + scan.header.stamp = node->now(); + scan.header.frame_id = "my_frame"; + // This three points are a match with the test dock template + scan.angle_min = -std::atan2(0.1, 0.9); + scan.angle_max = std::atan2(0.1, 0.9); + scan.angle_increment = std::atan2(0.1, 0.9); + scan.ranges = {0.9055, 1.0, 0.9055}; + scan.range_min = 0.9055; + scan.range_max = 1.0; + pub->publish(std::move(scan)); + rclcpp::spin_some(node->get_node_base_interface()); + + // Just expect that the pose is refined + // The actual values are not important for this test + // as they depend on the perception module and its already tested + pose.header.frame_id = "my_frame"; + EXPECT_TRUE(dock->getRefinedPose(pose)); + EXPECT_FALSE(dock->isDocked()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool success = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return success; +} diff --git a/scitos2_charging_dock/test/test_cluster.cpp b/scitos2_charging_dock/test/test_cluster.cpp new file mode 100644 index 0000000..4de9839 --- /dev/null +++ b/scitos2_charging_dock/test/test_cluster.cpp @@ -0,0 +1,81 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "scitos2_charging_dock/cluster.hpp" + +TEST(ScitosDockingCluster, getCentroid) { + scitos2_charging_dock::Cluster cluster; + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + cloud->push_back(pcl::PointXYZ(1.0, 2.0, 3.0)); + cloud->push_back(pcl::PointXYZ(4.0, 5.0, 6.0)); + cluster.cloud = cloud; + + auto centroid = cluster.getCentroid(); + + EXPECT_DOUBLE_EQ(2.5, centroid.x); + EXPECT_DOUBLE_EQ(3.5, centroid.y); + EXPECT_DOUBLE_EQ(4.5, centroid.z); +} + +TEST(ScitosDockingCluster, width) { + scitos2_charging_dock::Cluster cluster; + + // No cloud + EXPECT_DOUBLE_EQ(cluster.width(), 0.0); + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + cloud->push_back(pcl::PointXYZ(1.0, 2.0, 3.0)); + cloud->push_back(pcl::PointXYZ(4.0, 5.0, 6.0)); + cluster.cloud = cloud; + EXPECT_DOUBLE_EQ(cluster.width(), 4.2426406871192848); +} + +TEST(ScitosDockingCluster, valid) { + scitos2_charging_dock::Cluster cluster; + + // No cloud + EXPECT_FALSE(cluster.valid(0.0)); + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + cloud->push_back(pcl::PointXYZ(1.0, 2.0, 3.0)); + cloud->push_back(pcl::PointXYZ(4.0, 5.0, 6.0)); + cluster.cloud = cloud; + + EXPECT_FALSE(cluster.valid(0.0)); + EXPECT_TRUE(cluster.valid(5.0)); + EXPECT_FALSE(cluster.valid(10.0)); +} + +TEST(ScitosDockingCluster, operators) { + scitos2_charging_dock::Cluster cluster1; + cluster1.icp_score = 1.0; + scitos2_charging_dock::Cluster cluster2; + cluster2.icp_score = 2.0; + EXPECT_TRUE(cluster1 < cluster2); + EXPECT_FALSE(cluster1 > cluster2); + + cluster1.icp_score = 3.0; + cluster2.icp_score = 2.0; + EXPECT_FALSE(cluster1 < cluster2); + EXPECT_TRUE(cluster1 > cluster2); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + bool success = RUN_ALL_TESTS(); + return success; +} diff --git a/scitos2_charging_dock/test/test_dock_saver.cpp b/scitos2_charging_dock/test/test_dock_saver.cpp new file mode 100644 index 0000000..05c5ecf --- /dev/null +++ b/scitos2_charging_dock/test/test_dock_saver.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "nav2_util/node_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "scitos2_charging_dock/dock_saver.hpp" +#include "scitos2_msgs/srv/save_dock.hpp" + +class TestPublisher : public rclcpp::Node +{ +public: + TestPublisher() + : Node("scan_publisher") + { + scan_pub_ = create_publisher("scan", rclcpp::SensorDataQoS()); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(10), [this]() { + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = now(); + msg.header.frame_id = "my_frame"; + msg.angle_min = -M_PI; + msg.angle_max = M_PI; + msg.angle_increment = 2 * M_PI / 5; + msg.ranges = {1.0, 2.0, 3.0, 4.0, 5.0}; + msg.range_min = msg.ranges.front(); + msg.range_max = msg.ranges.back(); + scan_pub_->publish(msg); + }); + } + +protected: + rclcpp::Publisher::SharedPtr scan_pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +TEST(ScitosDockingSaver, saveDockEmpty) { + rclcpp::init(0, nullptr); + // Create the node + auto node = std::make_shared(); + + // Declare parameters for segmentation + nav2_util::declare_parameter_if_not_declared( + node, "dock_saver.segmentation.min_points", rclcpp::ParameterValue(10)); + nav2_util::declare_parameter_if_not_declared( + node, "dock_saver.segmentation.distance_threshold", rclcpp::ParameterValue(0.01)); + nav2_util::declare_parameter_if_not_declared( + node, "dock_saver.segmentation.min_width", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node, "dock_saver.segmentation.min_distance", rclcpp::ParameterValue(0.0)); + + node->configure(); + node->activate(); + + // Create the scan publisher node + auto pub_node = std::make_shared(); + auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);}); + + // Create the client service + auto req = std::make_shared(); + auto client = node->create_client("/dock_saver/save_dock"); + + // Wait for the service to be available + ASSERT_TRUE(client->wait_for_service()); + + // Call the service + auto result = client->async_send_request(req); + // Wait for the result + auto resp = std::make_shared(); + if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_INFO(node->get_logger(), "Service call successful"); + resp = result.get(); + } else { + RCLCPP_ERROR(node->get_logger(), "Service call failed"); + } + + // Check the response + EXPECT_FALSE(resp->result); + + // Cleaning up + node->deactivate(); + node->cleanup(); + node->shutdown(); + rclcpp::shutdown(); + pub_thread.join(); +} + +TEST(ScitosDockingSaver, saveDock) { + rclcpp::init(0, nullptr); + // Create the node + auto node = std::make_shared(); + + // Declare parameters for segmentation + nav2_util::declare_parameter_if_not_declared( + node, "dock_saver.segmentation.min_points", rclcpp::ParameterValue(0)); + nav2_util::declare_parameter_if_not_declared( + node, "dock_saver.segmentation.distance_threshold", rclcpp::ParameterValue(0.01)); + nav2_util::declare_parameter_if_not_declared( + node, "dock_saver.segmentation.min_width", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node, "dock_saver.segmentation.min_distance", rclcpp::ParameterValue(0.0)); + + node->configure(); + node->activate(); + + // Create the scan publisher node + auto pub_node = std::make_shared(); + auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);}); + + // Create the client service + auto req = std::make_shared(); + auto client = node->create_client("/dock_saver/save_dock"); + + // Wait for the service to be available + ASSERT_TRUE(client->wait_for_service()); + + // Call the service + auto result = client->async_send_request(req); + // Wait for the result + auto resp = std::make_shared(); + if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_INFO(node->get_logger(), "Service call successful"); + resp = result.get(); + } else { + RCLCPP_ERROR(node->get_logger(), "Service call failed"); + } + + // Check the response + EXPECT_TRUE(resp->result); + + // Cleaning up + node->deactivate(); + node->cleanup(); + node->shutdown(); + rclcpp::shutdown(); + pub_thread.join(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + bool success = RUN_ALL_TESTS(); + return success; +} diff --git a/scitos2_charging_dock/test/test_perception.cpp b/scitos2_charging_dock/test/test_perception.cpp new file mode 100644 index 0000000..9911118 --- /dev/null +++ b/scitos2_charging_dock/test/test_perception.cpp @@ -0,0 +1,375 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include +#include "nav2_util/node_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/LinearMath/Transform.h" +#include "scitos2_charging_dock/perception.hpp" + +class PerceptionFixture : public scitos2_charging_dock::Perception +{ +public: + PerceptionFixture( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & name, std::shared_ptr tf) + : Perception(node, name, tf) + { + } + + bool loadDockPointcloud(std::string filepath, scitos2_charging_dock::Pcloud & dock) + { + return scitos2_charging_dock::Perception::loadDockPointcloud(filepath, dock); + } + + scitos2_charging_dock::Clusters segmentsToClusters( + std::string frame_id, const scitos2_charging_dock::Segments & segments) + { + return scitos2_charging_dock::Perception::segmentsToClusters(frame_id, segments); + } + + tf2::Transform eigenToTransform(const Eigen::Matrix4f & T) + { + return scitos2_charging_dock::Perception::eigenToTransform(T); + } + + bool refineClusterPose( + scitos2_charging_dock::Cluster & cluster, + scitos2_charging_dock::Pcloud::ConstPtr cloud_template) + { + return scitos2_charging_dock::Perception::refineClusterPose(cluster, cloud_template); + } + + bool refineAllClustersPoses( + scitos2_charging_dock::Clusters & clusters, + scitos2_charging_dock::Pcloud::ConstPtr cloud_template, + geometry_msgs::msg::PoseStamped & dock_pose) + { + return scitos2_charging_dock::Perception::refineAllClustersPoses( + clusters, cloud_template, dock_pose); + } +}; + +geometry_msgs::msg::Point create_point(double x, double y) +{ + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + return point; +} + +TEST(ScitosDockingPerception, configure) { + // Create a node + auto node = std::make_shared("perception_test"); + + // Declare debug parameter + node->declare_parameter("test.perception.enable_debug", rclcpp::ParameterValue(true)); + + // Create the perception object + auto perception = std::make_shared(node, "test", nullptr); +} + +TEST(ScitosDockingPerception, dynamicParameters) { + // Create a node + auto node = std::make_shared("perception_test"); + auto perception = std::make_shared(node, "test", nullptr); + + // Activate the node + node->configure(); + node->activate(); + + auto params = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + // Set the parameters + auto results = params->set_parameters_atomically( + {rclcpp::Parameter("test.perception.icp_max_iter", 5), + rclcpp::Parameter("test.perception.icp_min_score", 0.5), + rclcpp::Parameter("test.perception.icp_max_corr_dis", 0.5), + rclcpp::Parameter("test.perception.icp_max_trans_eps", 0.5), + rclcpp::Parameter("test.perception.icp_max_eucl_fit_eps", 0.5), + rclcpp::Parameter("test.perception.enable_debug", false)}); + + // Spin + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + + // Check parameters + EXPECT_EQ(node->get_parameter("test.perception.icp_max_iter").as_int(), 5); + EXPECT_DOUBLE_EQ(node->get_parameter("test.perception.icp_min_score").as_double(), 0.5); + EXPECT_DOUBLE_EQ(node->get_parameter("test.perception.icp_max_corr_dis").as_double(), 0.5); + EXPECT_DOUBLE_EQ(node->get_parameter("test.perception.icp_max_trans_eps").as_double(), 0.5); + EXPECT_DOUBLE_EQ(node->get_parameter("test.perception.icp_max_eucl_fit_eps").as_double(), 0.5); + EXPECT_EQ(node->get_parameter("test.perception.enable_debug").as_bool(), false); + + // Cleaning up + node->deactivate(); + node->cleanup(); + node->shutdown(); +} + +TEST(ScitosDockingPerception, loadDockPointcloud) { + // Create a node + auto node = std::make_shared("perception_test"); + auto perception = std::make_shared(node, "test", nullptr); + + // Try to load an empty file + std::string filename = ""; + scitos2_charging_dock::Pcloud dock; + bool success = perception->loadDockPointcloud(filename, dock); + // Check if the docking station was loaded + EXPECT_FALSE(success); + EXPECT_EQ(dock.size(), 0); + + // Try to load another docking station + std::string pkg = ament_index_cpp::get_package_share_directory("scitos2_charging_dock"); + std::string path = pkg + "/test/empty_dock_test.pcd"; + success = perception->loadDockPointcloud(path, dock); + // Check if the docking station was loaded + EXPECT_FALSE(success); + EXPECT_EQ(dock.size(), 0); + + // Try to load a valid docking station + path = pkg + "/test/dock_test.pcd"; + success = perception->loadDockPointcloud(path, dock); + // Check if the docking station was loaded + EXPECT_TRUE(success); + EXPECT_EQ(dock.size(), 3); +} + +TEST(ScitosDockingPerception, storeDockPointcloud) { + // Create a node + auto node = std::make_shared("perception_test"); + auto perception = std::make_shared(node, "test", nullptr); + + // Store a pointcloud + std::string pkg = ament_index_cpp::get_package_share_directory("scitos2_charging_dock"); + std::string path = pkg + "/test/empty_dock_test2.pcd"; + scitos2_charging_dock::Pcloud dock; + dock.push_back(pcl::PointXYZ(0, 0, 0)); + bool success = perception->storeDockPointcloud(path, dock); + + // Check if the docking station was stored + EXPECT_TRUE(success); +} + +TEST(ScitosDockingPerception, segmentsToClusters) { + // Create a node + auto node = std::make_shared("perception_test"); + auto perception = std::make_shared(node, "test", nullptr); + + // Create a segment + scitos2_charging_dock::Segment segment; + segment.points.push_back(create_point(0, 0)); + segment.points.push_back(create_point(1, 0)); + segment.points.push_back(create_point(0, 1)); + scitos2_charging_dock::Segments segments; + segments.push_back(segment); + + // Create another segment + segment.points.clear(); + segment.points.push_back(create_point(2, 0)); + segment.points.push_back(create_point(3, 0)); + segment.points.push_back(create_point(2, 1)); + segment.points.push_back(create_point(2, 5)); + segments.push_back(segment); + + // Convert the segments to clusters + auto clusters = perception->segmentsToClusters("test_link", segments); + + // Check if the clusters were created + EXPECT_EQ(clusters.size(), 2); + EXPECT_EQ(clusters[0].cloud->size(), 3); + EXPECT_EQ(clusters[0].cloud->header.frame_id, "test_link"); + EXPECT_EQ(clusters[0].cloud->points[0].x, 0); + EXPECT_EQ(clusters[0].cloud->points[0].y, 0); + EXPECT_EQ(clusters[0].cloud->points[1].x, 1); + EXPECT_EQ(clusters[0].cloud->points[1].y, 0); + EXPECT_EQ(clusters[0].cloud->points[2].x, 0); + EXPECT_EQ(clusters[0].cloud->points[2].y, 1); + EXPECT_EQ(clusters[1].cloud->size(), 4); + EXPECT_EQ(clusters[1].cloud->points[0].x, 2); + EXPECT_EQ(clusters[1].cloud->points[0].y, 0); + EXPECT_EQ(clusters[1].cloud->points[1].x, 3); + EXPECT_EQ(clusters[1].cloud->points[1].y, 0); + EXPECT_EQ(clusters[1].cloud->points[2].x, 2); + EXPECT_EQ(clusters[1].cloud->points[2].y, 1); + EXPECT_EQ(clusters[1].cloud->points[3].x, 2); + EXPECT_EQ(clusters[1].cloud->points[3].y, 5); + EXPECT_EQ(clusters[1].cloud->header.frame_id, "test_link"); +} + +TEST(ScitosDockingPerception, eigenToTransform) { + // Create a node + auto node = std::make_shared("perception_test"); + auto perception = std::make_shared(node, "test", nullptr); + + // Create a transformation matrix + Eigen::Matrix4f T = Eigen::Matrix4f::Identity(); + T(0, 0) = 1; + T(0, 1) = 2; + T(0, 2) = 3; + T(0, 3) = 4; + T(1, 0) = 5; + T(1, 1) = 6; + T(1, 2) = 7; + T(1, 3) = 8; + T(2, 0) = 9; + T(2, 1) = 10; + T(2, 2) = 11; + T(2, 3) = 12; + T(3, 0) = 13; + T(3, 1) = 14; + T(3, 2) = 15; + T(3, 3) = 16; + + // Convert the matrix to a transform + auto transform = perception->eigenToTransform(T); + + // Check if the transform was created + EXPECT_EQ(transform.getOrigin().getX(), 4); + EXPECT_EQ(transform.getOrigin().getY(), 8); + EXPECT_EQ(transform.getOrigin().getZ(), 12); + EXPECT_EQ(transform.getBasis().getRow(0).getX(), 1); + EXPECT_EQ(transform.getBasis().getRow(0).getY(), 2); + EXPECT_EQ(transform.getBasis().getRow(0).getZ(), 3); + EXPECT_EQ(transform.getBasis().getRow(1).getX(), 5); + EXPECT_EQ(transform.getBasis().getRow(1).getY(), 6); + EXPECT_EQ(transform.getBasis().getRow(1).getZ(), 7); + EXPECT_EQ(transform.getBasis().getRow(2).getX(), 9); + EXPECT_EQ(transform.getBasis().getRow(2).getY(), 10); + EXPECT_EQ(transform.getBasis().getRow(2).getZ(), 11); +} + +TEST(ScitosDockingPerception, refineClusterToPose) { + // Create a node + auto node = std::make_shared("perception_test"); + auto perception = std::make_shared(node, "test", nullptr); + + // Create a cluster + scitos2_charging_dock::Cluster cluster; + cluster.cloud = std::make_shared>(); + cluster.cloud->push_back(pcl::PointXYZ(0, 0, 0)); + cluster.cloud->push_back(pcl::PointXYZ(1, 0, 0)); + cluster.cloud->push_back(pcl::PointXYZ(1, 1.5, 0)); + cluster.cloud->push_back(pcl::PointXYZ(1, 2, 0)); + + // Create a template + auto cloud_template = std::make_shared(); + cloud_template->push_back(pcl::PointXYZ(0, 0, 0)); + cloud_template->push_back(pcl::PointXYZ(1, 0, 0)); + cloud_template->push_back(pcl::PointXYZ(1, 1.5, 0)); + cloud_template->push_back(pcl::PointXYZ(1, 2, 0)); + + // Set the initial pose + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = 1; + initial_pose.position.y = 1; + + // Refine the cluster pose + perception->setInitialEstimate(initial_pose, "test_link"); + bool success = perception->refineClusterPose(cluster, cloud_template); + + // Check if the pose was refined + EXPECT_TRUE(success); + EXPECT_EQ(cluster.icp_pose.header.frame_id, "test_link"); + EXPECT_NEAR(cluster.icp_pose.pose.position.x, 1.0, 0.01); + EXPECT_NEAR(cluster.icp_pose.pose.position.y, 1.0, 0.01); + + // Now set another cluster different from the template + scitos2_charging_dock::Cluster cluster2; + cluster2.cloud = std::make_shared>(); + cluster2.cloud->push_back(pcl::PointXYZ(40, 50, 7)); + cluster2.cloud->push_back(pcl::PointXYZ(10, 30, 0)); + cluster2.cloud->push_back(pcl::PointXYZ(10, 1.5, 0)); + cluster2.cloud->push_back(pcl::PointXYZ(10, 90, 0)); + + // Refine the cluster pose + perception->setInitialEstimate(initial_pose, "test_link"); + success = perception->refineClusterPose(cluster2, cloud_template); + + // Check if the pose was refined + EXPECT_FALSE(success); +} + +TEST(ScitosDockingPerception, refineAllClustersPoses) { + // Create a node + auto node = std::make_shared("perception_test"); + + // Create the TF + auto tf_buffer = std::make_shared(node->get_clock()); + + // Set debug mode + nav2_util::declare_parameter_if_not_declared( + node, "test.perception.enable_debug", rclcpp::ParameterValue(true)); + node->configure(); + + // Create the perception module + auto perception = std::make_shared(node, "test", tf_buffer); + + // Create two clusters + scitos2_charging_dock::Clusters clusters; + scitos2_charging_dock::Cluster cluster; + cluster.cloud = std::make_shared>(); + cluster.cloud->header.frame_id = "test_link"; + cluster.cloud->push_back(pcl::PointXYZ(0, 0, 0)); + cluster.cloud->push_back(pcl::PointXYZ(1, 0, 0)); + cluster.cloud->push_back(pcl::PointXYZ(1, 1.5, 0)); + cluster.cloud->push_back(pcl::PointXYZ(1, 2, 0)); + clusters.push_back(cluster); + + scitos2_charging_dock::Cluster cluster2; + cluster2.cloud = std::make_shared>(); + cluster2.cloud->header.frame_id = "test_link"; + cluster2.cloud->push_back(pcl::PointXYZ(2, 0, 0)); + cluster2.cloud->push_back(pcl::PointXYZ(3, 0, 0)); + cluster2.cloud->push_back(pcl::PointXYZ(2, 1, 0)); + cluster2.cloud->push_back(pcl::PointXYZ(2, 5, 0)); + clusters.push_back(cluster2); + + // Create a template + auto cloud_template = std::make_shared(); + cloud_template->header.frame_id = "test_link"; + cloud_template->push_back(pcl::PointXYZ(0, 0, 0)); + cloud_template->push_back(pcl::PointXYZ(1, 0, 0)); + cloud_template->push_back(pcl::PointXYZ(1, 1.5, 0)); + cloud_template->push_back(pcl::PointXYZ(1, 2, 0)); + + // Set the initial pose + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = 0.0; + initial_pose.position.y = 0.0; + perception->setInitialEstimate(initial_pose, "test_link"); + + // Refine the clusters poses + geometry_msgs::msg::PoseStamped dock_pose; + bool success = perception->refineAllClustersPoses(clusters, cloud_template, dock_pose); + + // Check if the dock is found + EXPECT_TRUE(success); + EXPECT_EQ(dock_pose.header.frame_id, "test_link"); + EXPECT_NEAR(dock_pose.pose.position.x, 0.0, 0.01); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool success = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return success; +} diff --git a/scitos2_charging_dock/test/test_segmentation.cpp b/scitos2_charging_dock/test/test_segmentation.cpp new file mode 100644 index 0000000..8665839 --- /dev/null +++ b/scitos2_charging_dock/test/test_segmentation.cpp @@ -0,0 +1,400 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "scitos2_charging_dock/segmentation.hpp" + +class SegmentationFixture : public scitos2_charging_dock::Segmentation +{ +public: + explicit SegmentationFixture( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & name) + : Segmentation(node, name) + { + } + + geometry_msgs::msg::Point fromPolarToCartesian(double range, double angle) + { + return scitos2_charging_dock::Segmentation::fromPolarToCartesian(range, angle); + } + + std::vector scanToPoints(const sensor_msgs::msg::LaserScan & scan) + { + return scitos2_charging_dock::Segmentation::scanToPoints(scan); + } + + double euclideanDistance( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) + { + return scitos2_charging_dock::Segmentation::euclideanDistance(p1, p2); + } + + bool isJumpBetweenPoints( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, double threshold) + { + return scitos2_charging_dock::Segmentation::isJumpBetweenPoints(p1, p2, threshold); + } + + void setDistanceThreshold(double distance_threshold) + { + distance_threshold_ = distance_threshold; + } + + void setFilteringParams( + int min_points_segment, int max_points_segment, + double min_avg_distance_from_sensor, double max_avg_distance_from_sensor, + double min_segment_width, double max_segment_width) + { + min_points_segment_ = min_points_segment; + max_points_segment_ = max_points_segment; + min_avg_distance_from_sensor_ = min_avg_distance_from_sensor; + max_avg_distance_from_sensor_ = max_avg_distance_from_sensor; + min_segment_width_ = min_segment_width; + max_segment_width_ = max_segment_width; + } + + double getMinPointsSegment() {return min_points_segment_;} + double getMaxPointsSegment() {return max_points_segment_;} + double getMinAvgDistanceFromSensor() {return min_avg_distance_from_sensor_;} + double getMaxAvgDistanceFromSensor() {return max_avg_distance_from_sensor_;} + double getMinSegmentWidth() {return min_segment_width_;} + double getMaxSegmentWidth() {return max_segment_width_;} +}; + +geometry_msgs::msg::Point create_point(double x, double y) +{ + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + return point; +} + +// Create a vector of points with a distance between them +std::vector create_points(int num_points, double distance) +{ + std::vector points; + for (int i = 0; i < num_points; i++) { + geometry_msgs::msg::Point point; + point.x = i * distance; + point.y = i * distance; + points.push_back(point); + } + return points; +} + +// Create a laser scan with a distance between the points +sensor_msgs::msg::LaserScan create_scan(int num_points, double distance) +{ + sensor_msgs::msg::LaserScan scan; + scan.angle_min = -M_PI; + scan.angle_max = M_PI; + scan.angle_increment = 2 * M_PI / distance; + for (int i = 0; i < num_points; i++) { + double range = std::hypot(i * distance, i * distance); + scan.ranges.push_back(range); + } + scan.range_min = scan.ranges.front(); + scan.range_max = scan.ranges.back(); + return scan; +} + +TEST(SegmentationTest, dynamicParameters) { + // Create a node + auto node = std::make_shared("segmentation_test"); + auto segmentation = std::make_shared(node, "test"); + + // Activate the node + node->configure(); + node->activate(); + + auto params = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + // Set the parameters + auto results = params->set_parameters_atomically( + {rclcpp::Parameter("test.segmentation.min_points", 1), + rclcpp::Parameter("test.segmentation.max_points", 3), + rclcpp::Parameter("test.segmentation.distance_threshold", 1.0), + rclcpp::Parameter("test.segmentation.min_distance", 10.0), + rclcpp::Parameter("test.segmentation.max_distance", 0.1), + rclcpp::Parameter("test.segmentation.min_width", 10.0), + rclcpp::Parameter("test.segmentation.max_width", 0.1)}); + + // Spin + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + + // Check parameters + EXPECT_EQ(node->get_parameter("test.segmentation.min_points").as_int(), 1); + EXPECT_EQ(node->get_parameter("test.segmentation.max_points").as_int(), 3); + EXPECT_EQ(node->get_parameter("test.segmentation.distance_threshold").as_double(), 1.0); + EXPECT_EQ(node->get_parameter("test.segmentation.min_distance").as_double(), 10.0); + EXPECT_EQ(node->get_parameter("test.segmentation.max_distance").as_double(), 0.1); + EXPECT_EQ(node->get_parameter("test.segmentation.min_width").as_double(), 10.0); + EXPECT_EQ(node->get_parameter("test.segmentation.max_width").as_double(), 0.1); + + // Cleaning up + node->deactivate(); + node->cleanup(); + node->shutdown(); +} + +TEST(SegmentationTest, euclideanDistance) { + // Create a node + auto node = std::make_shared("segmentation_test"); + auto segmentation = std::make_shared(node, "test"); + + // Calculate the euclidean distance between two points + geometry_msgs::msg::Point point1; + point1.x = 1.0; + point1.y = 1.0; + geometry_msgs::msg::Point point2; + point2.x = 3.0; + point2.y = 3.0; + double distance = segmentation->euclideanDistance(point1, point2); + // Check the distance + EXPECT_DOUBLE_EQ(distance, 2.8284271247461903); +} + +TEST(SegmentationTest, fromPolarToCartesian) { + // Create a node + auto node = std::make_shared("segmentation_test"); + auto segmentation = std::make_shared(node, "test"); + + // Convert a polar point to a cartesian point + geometry_msgs::msg::Point point = segmentation->fromPolarToCartesian(1.0, 0.0); + // Check the point + EXPECT_DOUBLE_EQ(point.x, 1.0); + EXPECT_DOUBLE_EQ(point.y, 0.0); + + // Convert a polar point to a cartesian point + point = segmentation->fromPolarToCartesian(1.0, M_PI / 2); + // Check the point + EXPECT_NEAR(point.x, 0.0, 1e-3); + EXPECT_DOUBLE_EQ(point.y, 1.0); +} + +TEST(SegmentationTest, scanToPoints) { + // Create a node + auto node = std::make_shared("segmentation_test"); + auto segmentation = std::make_shared(node, "test"); + + // Convert a laser scan to a vector of points + sensor_msgs::msg::LaserScan scan; + scan.angle_min = -M_PI; + scan.angle_max = M_PI; + scan.angle_increment = 2 * M_PI / 5; + scan.ranges = {1.0, 2.0, 3.0, 4.0, 5.0}; + scan.range_min = scan.ranges.front(); + scan.range_max = scan.ranges.back(); + scan.ranges.push_back(0.0); + scan.ranges.push_back(7.0); + auto points = segmentation->scanToPoints(scan); + // Check the number of points as the last two points are out of range + EXPECT_EQ(points.size(), 5); + // Check the points + double phi = scan.angle_min; + for (int i = 0; i < 5; i++) { + EXPECT_DOUBLE_EQ(points[i].x, scan.ranges[i] * cos(phi)); + EXPECT_DOUBLE_EQ(points[i].y, scan.ranges[i] * sin(phi)); + phi += scan.angle_increment; + } +} + +TEST(SegmentationTest, isJumpBetweenPoints) { + // Create a node + auto node = std::make_shared("segmentation_test"); + auto segmentation = std::make_shared(node, "test"); + + // Create two points with a distance of 1.0 + geometry_msgs::msg::Point point1; + point1.x = 0.0; + point1.y = 0.0; + geometry_msgs::msg::Point point2; + point2.x = 1.0; + point2.y = 1.0; + + // Check if there is a jump between two points + bool jump = segmentation->isJumpBetweenPoints(point1, point2, 0.1); + // Check the result + EXPECT_TRUE(jump); + + // Check if there is a jump between two points + jump = segmentation->isJumpBetweenPoints(point1, point2, 2.0); + // Check the result + EXPECT_FALSE(jump); +} + +TEST(SegmentationTest, performSegmentation) { + // Create a node + auto node = std::make_shared("segmentation_test"); + auto segmentation = std::make_shared(node, "test"); + + // Create an empty scan + sensor_msgs::msg::LaserScan fake_scan; + scitos2_charging_dock::Segments fake_segments; + EXPECT_FALSE(segmentation->performSegmentation(fake_scan, fake_segments)); + + // Set the jump distance to 0.1 + segmentation->setDistanceThreshold(0.1); + // Create a scan with points with a distance of 0.05 + auto scan = create_scan(3, 0.05); + + // Perform the segmentation + scitos2_charging_dock::Segments segments; + EXPECT_TRUE(segmentation->performSegmentation(scan, segments)); + // Check the number of segments + // Should be 1 segment as the jump distance is 0.1 + // and the distance between points is 0.05 + EXPECT_EQ(segments.size(), 1); + // Check the number of points in the segment + // Should be 3 points in the segment + EXPECT_EQ(segments.front().size(), 3); + + // Now we set the points with a distance of 1.0 + scan = create_scan(3, 1.0); + // Perform the segmentation + segments.clear(); + EXPECT_TRUE(segmentation->performSegmentation(scan, segments)); + // Check the number of segments + // Should be 1 segment per point as the jump distance is 0.1 + // and the distance between points is 1.0 + EXPECT_EQ(segments.size(), 3); + // Check the number of points in the segment + for (auto segment : segments) { + EXPECT_EQ(segment.size(), 1); + } +} + +TEST(SegmentationTest, filteringSegments) { + // Create a node + auto node = std::make_shared("segmentation_test"); + auto segmentation = std::make_shared(node, "test"); + + // Set the filtering parameters + segmentation->setFilteringParams(1, 3, 1.0, 10.0, 0.1, 10.0); + + // Set a segment list with 0 segments + std::vector segment_list; + auto filtered_segments = segmentation->filterSegments(segment_list); + // Check the filtered segments + EXPECT_EQ(filtered_segments.size(), 0); + + // Set a segment list with 4 segments of 1 point + segment_list.clear(); + scitos2_charging_dock::Segment segment; + segment.points.push_back(create_point(0.0, 0.0)); + segment_list.push_back(segment); + segment_list.push_back(segment); + segment_list.push_back(segment); + segment_list.push_back(segment); + // Filter the segments + filtered_segments = segmentation->filterSegments(segment_list); + // Check the filtered segments + EXPECT_EQ(filtered_segments.size(), 0); + + // Set a segment list with 2 segments with centroid below the minimum distance + segment_list.clear(); + segment.clear(); + segment.points.push_back(create_point(0.0, 0.0)); + segment_list.push_back(segment); + segment.clear(); + segment.points.push_back(create_point(0.5, 0.5)); + segment_list.push_back(segment); + // Filter the segments + filtered_segments = segmentation->filterSegments(segment_list); + // Check the filtered segments + EXPECT_EQ(filtered_segments.size(), 0); + + // Set a segment list with 2 segments with centroid above the maximum distance + segment_list.clear(); + segment.clear(); + segment.points.push_back(create_point(11.0, 11.0)); + segment_list.push_back(segment); + segment.clear(); + segment.points.push_back(create_point(12.0, 12.0)); + segment_list.push_back(segment); + // Filter the segments + filtered_segments = segmentation->filterSegments(segment_list); + // Check the filtered segments + EXPECT_EQ(filtered_segments.size(), 0); + + // Set a segment list with 2 segments with width below the minimum width + segment_list.clear(); + segment.clear(); + segment.points.push_back(create_point(0.0, 0.0)); + segment.points.push_back(create_point(0.0, 0.0)); + segment_list.push_back(segment); + segment.clear(); + segment.points.push_back(create_point(1.0, 1.0)); + segment.points.push_back(create_point(1.0, 1.0)); + segment_list.push_back(segment); + // Filter the segments + filtered_segments = segmentation->filterSegments(segment_list); + // Check the filtered segments + EXPECT_EQ(filtered_segments.size(), 0); + + // Set a segment list with 2 segments with width above the maximum width + segment_list.clear(); + segment.clear(); + segment.points.push_back(create_point(0.0, 0.0)); + segment.points.push_back(create_point(10.0, 10.0)); + segment_list.push_back(segment); + segment.clear(); + segment.points.push_back(create_point(0.0, 0.0)); + segment.points.push_back(create_point(15.0, 15.0)); + segment_list.push_back(segment); + // Filter the segments + filtered_segments = segmentation->filterSegments(segment_list); + // Check the filtered segments + EXPECT_EQ(filtered_segments.size(), 0); + + // Set a segment list with several segments + segment_list.clear(); + segment.clear(); + segment.points.push_back(create_point(0.0, 0.0)); + segment_list.push_back(segment); + segment.points.push_back(create_point(0.0, 0.0)); + segment.points.push_back(create_point(1.0, 1.0)); + segment_list.push_back(segment); + segment.clear(); + segment.points.push_back(create_point(2.0, 2.0)); + segment.points.push_back(create_point(3.0, 3.0)); + segment.points.push_back(create_point(4.0, 4.0)); + segment_list.push_back(segment); + segment.clear(); + segment.points.push_back(create_point(5.0, 5.0)); + segment.points.push_back(create_point(6.0, 6.0)); + segment.points.push_back(create_point(7.0, 7.0)); + segment.points.push_back(create_point(8.0, 8.0)); + segment_list.push_back(segment); + // Filter the segments + filtered_segments = segmentation->filterSegments(segment_list); + // Check the filtered segments + EXPECT_EQ(filtered_segments.size(), 1); + EXPECT_EQ(filtered_segments[0].centroid().x, 3.0); + EXPECT_EQ(filtered_segments[0].centroid().y, 3.0); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool success = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return success; +} diff --git a/scitos2_common/CHANGELOG.rst b/scitos2_common/CHANGELOG.rst index 63327f8..c98fb2d 100644 --- a/scitos2_common/CHANGELOG.rst +++ b/scitos2_common/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package scitos2_common -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2.0.0 (01-04-2024) ------------------ diff --git a/scitos2_core/CHANGELOG.rst b/scitos2_core/CHANGELOG.rst index f748dc5..029b572 100644 --- a/scitos2_core/CHANGELOG.rst +++ b/scitos2_core/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package scitos2_core -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2.0.0 (XX-04-2024) ------------------ diff --git a/scitos2_mira/CHANGELOG.rst b/scitos2_mira/CHANGELOG.rst index 0eacb12..1a2b0ea 100644 --- a/scitos2_mira/CHANGELOG.rst +++ b/scitos2_mira/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package scitos2_mira -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2.0.0 (XX-04-2024) ------------------ diff --git a/scitos2_mira/include/scitos2_mira/mira_framework.hpp b/scitos2_mira/include/scitos2_mira/mira_framework.hpp index 563f7f0..00107e4 100644 --- a/scitos2_mira/include/scitos2_mira/mira_framework.hpp +++ b/scitos2_mira/include/scitos2_mira/mira_framework.hpp @@ -49,23 +49,22 @@ class MiraFramework : public nav2_util::LifecycleNode using ModuleMap = std::unordered_map; /** - * @brief Construct a new laser Segmentation object + * @brief Construct a new Mira Framework object * * @param options Node options */ explicit MiraFramework(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** - * @brief Destroy the laser Segmentation object + * @brief Destroy the Mira Framework object * */ ~MiraFramework(); protected: /** - * @brief Configures the modules parameters and member variables + * @brief Configures the modules plugin. * - * Configures modules plugin. * @param state LifeCycle Node's state * @return Success or Failure * @throw pluginlib::PluginlibException When failed to initialize module @@ -74,18 +73,16 @@ class MiraFramework : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; /** - * @brief Activates member variables + * @brief Activates the modules. * - * Activates the modules * @param state LifeCycle Node's state * @return Success or Failure */ nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; /** - * @brief Deactivates member variables + * @brief Deactivates the modules. * - * Deactivates the modules. * @param state LifeCycle Node's state * @return Success or Failure */ @@ -94,15 +91,14 @@ class MiraFramework : public nav2_util::LifecycleNode /** * @brief Calls clean up states and resets member variables. * - * module clean up state is called, and resets rest of the - * variables * @param state LifeCycle Node's state * @return Success or Failure */ nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; /** - * @brief Called when in Shutdown state + * @brief Called when in Shutdown state. + * * @param state LifeCycle Node's state * @return Success or Failure */ @@ -120,7 +116,7 @@ class MiraFramework : public nav2_util::LifecycleNode std::unique_ptr framework_; bool loaded_; - // Modules Plugins + // Module Plugins pluginlib::ClassLoader module_loader_; ModuleMap modules_; std::vector default_ids_; diff --git a/scitos2_modules/CHANGELOG.rst b/scitos2_modules/CHANGELOG.rst index 98f6179..4d610fe 100644 --- a/scitos2_modules/CHANGELOG.rst +++ b/scitos2_modules/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package scitos2_modules -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2.0.0 (XX-04-2024) ------------------ diff --git a/scitos2_msgs/CHANGELOG.rst b/scitos2_msgs/CHANGELOG.rst index 1f00375..c140795 100644 --- a/scitos2_msgs/CHANGELOG.rst +++ b/scitos2_msgs/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package scitos2_msgs -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2.0.0 (XX-03-2024) ------------------ @@ -8,6 +8,7 @@ Changelog for package scitos2_msgs * Rename package to scitos2_msgs. * Added emergency_stop_status to EmergencyStopStatus.msg. * Added bumper_status to BumperStatus.msg. +* Added SaveDock.srv. 1.4.0 (25-04-2023) ------------------ diff --git a/scitos2_msgs/CMakeLists.txt b/scitos2_msgs/CMakeLists.txt index 50c1aa5..fcd5fbb 100644 --- a/scitos2_msgs/CMakeLists.txt +++ b/scitos2_msgs/CMakeLists.txt @@ -46,6 +46,7 @@ set(srv_files "srv/ResetBarrierStop.srv" "srv/ResetMotorStop.srv" "srv/ResetOdometry.srv" + "srv/SaveDock.srv" "srv/SavePersistentErrors.srv" "srv/SuspendBumper.srv" ) diff --git a/scitos2_msgs/README.md b/scitos2_msgs/README.md index bb64f80..f641edf 100644 --- a/scitos2_msgs/README.md +++ b/scitos2_msgs/README.md @@ -19,6 +19,7 @@ This package contains messages and services for the Metralabs Scitos robot base. * [EmergencyStop](srv/EmergencyStop.srv): Service to perform an emergency stop and set the motor emergency stop flag. * [EnableMotors](srv/EnableMotors.srv): Service to enable or disable the motors. * [EnableRfid](srv/EnableRfid.srv): Service to enable or disable the RFID reader. +* [SaveDock](srv/SaveDock.srv): Service to record the save the current dock pointcloud as a PCD file. * [ResetBarrierStop](srv/ResetBarrierStop.srv): Service to reset the magnetic barrier stop flag. * [ResetMotorStop](srv/ResetMotorStop.srv): Service to reset the motor stop flags (bumper, emergency stop flags, etc). * [ResetOdometry](srv/ResetOdometry.srv): Service to reset the odometry to zero. diff --git a/scitos2_msgs/srv/SaveDock.srv b/scitos2_msgs/srv/SaveDock.srv new file mode 100644 index 0000000..78079d0 --- /dev/null +++ b/scitos2_msgs/srv/SaveDock.srv @@ -0,0 +1,7 @@ +# Save the current dock +# Can be an absolute path to a file: file:///path/to/stations/dock1.pcd +# Or, relative to a ROS package: package://my_ros_package/stations/dock1.yaml +string scan_topic +string dock_url +--- +bool result