From cc5351a266399199c449828dcb351301c4253117 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Wed, 5 Jul 2023 09:59:25 +0900 Subject: [PATCH] Add search helper for goal angle tolerance (#317) --- mbf_costmap_nav/CMakeLists.txt | 21 + .../costmap_navigation_server.h | 84 +-- .../mbf_costmap_nav/footprint_helper.h | 13 +- .../mbf_costmap_nav/free_pose_search.h | 203 +++++++ .../mbf_costmap_nav/free_pose_search_viz.h | 100 ++++ mbf_costmap_nav/package.xml | 3 + .../costmap_navigation_server.cpp | 270 ++++++---- .../src/mbf_costmap_nav/footprint_helper.cpp | 96 ++++ .../src/mbf_costmap_nav/free_pose_search.cpp | 359 +++++++++++++ .../mbf_costmap_nav/free_pose_search_viz.cpp | 127 +++++ mbf_costmap_nav/test/config.yaml | 7 + mbf_costmap_nav/test/free_pose_search.test | 22 + .../test/free_pose_search_test.cpp | 506 ++++++++++++++++++ mbf_costmap_nav/test/record_bag_file.sh | 10 + mbf_msgs/CMakeLists.txt | 1 + 15 files changed, 1684 insertions(+), 138 deletions(-) create mode 100644 mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search.h create mode 100644 mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search_viz.h create mode 100644 mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search.cpp create mode 100644 mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search_viz.cpp create mode 100644 mbf_costmap_nav/test/config.yaml create mode 100644 mbf_costmap_nav/test/free_pose_search.test create mode 100644 mbf_costmap_nav/test/free_pose_search_test.cpp create mode 100755 mbf_costmap_nav/test/record_bag_file.sh diff --git a/mbf_costmap_nav/CMakeLists.txt b/mbf_costmap_nav/CMakeLists.txt index 3ebc375b..ea24d26f 100644 --- a/mbf_costmap_nav/CMakeLists.txt +++ b/mbf_costmap_nav/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(mbf_costmap_nav) +set(CMAKE_CXX_STANDARD 17) + find_package(catkin REQUIRED COMPONENTS costmap_2d @@ -76,6 +78,8 @@ add_library(${MBF_COSTMAP_2D_SERVER_LIB} src/mbf_costmap_nav/costmap_recovery_execution.cpp src/mbf_costmap_nav/costmap_wrapper.cpp src/mbf_costmap_nav/footprint_helper.cpp + src/mbf_costmap_nav/free_pose_search.cpp + src/mbf_costmap_nav/free_pose_search_viz.cpp ) add_dependencies(${MBF_COSTMAP_2D_SERVER_LIB} ${catkin_EXPORTED_TARGETS}) add_dependencies(${MBF_COSTMAP_2D_SERVER_LIB} ${MBF_NAV_CORE_WRAPPER_LIB}) @@ -108,3 +112,20 @@ install(DIRECTORY include/${PROJECT_NAME}/ catkin_install_python(PROGRAMS scripts/move_base_legacy_relay.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +############# +## Test ## +############# + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + find_package(map_server REQUIRED) + + add_rostest_gtest(free_pose_search_test + test/free_pose_search.test + test/free_pose_search_test.cpp + ) + target_link_libraries(free_pose_search_test + ${MBF_COSTMAP_2D_SERVER_LIB} + ) +endif() diff --git a/mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h b/mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h index dbc0ce44..4621406f 100644 --- a/mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h +++ b/mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h @@ -47,6 +47,7 @@ #include #include #include +#include #include #include @@ -71,8 +72,8 @@ namespace mbf_costmap_nav * @brief Classes belonging to the Move Base Server level. */ - -typedef boost::shared_ptr > DynamicReconfigureServerCostmapNav; +typedef boost::shared_ptr > + DynamicReconfigureServerCostmapNav; /// @brief A mapping from a string to a map-ptr. typedef boost::unordered_map StringToMap; @@ -88,14 +89,13 @@ typedef boost::unordered_map StringToMap; class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServer { public: - typedef boost::shared_ptr Ptr; /** * @brief Constructor * @param tf_listener_ptr Shared pointer to a common TransformListener */ - CostmapNavigationServer(const TFPtr &tf_listener_ptr); + CostmapNavigationServer(const TFPtr& tf_listener_ptr); /** * @brief Destructor @@ -105,16 +105,14 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe virtual void stop(); private: - /** * @brief Create a new planner execution. * @param plugin_name Name of the planner to use. * @param plugin_ptr Shared pointer to the plugin to use. * @return Shared pointer to a new @ref planner_execution "PlannerExecution". */ - virtual mbf_abstract_nav::AbstractPlannerExecution::Ptr newPlannerExecution( - const std::string &plugin_name, - const mbf_abstract_core::AbstractPlanner::Ptr &plugin_ptr); + virtual mbf_abstract_nav::AbstractPlannerExecution::Ptr + newPlannerExecution(const std::string& plugin_name, const mbf_abstract_core::AbstractPlanner::Ptr& plugin_ptr); /** * @brief Create a new controller execution. @@ -122,9 +120,8 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe * @param plugin_ptr Shared pointer to the plugin to use. * @return Shared pointer to a new @ref controller_execution "ControllerExecution". */ - virtual mbf_abstract_nav::AbstractControllerExecution::Ptr newControllerExecution( - const std::string &plugin_name, - const mbf_abstract_core::AbstractController::Ptr &plugin_ptr); + virtual mbf_abstract_nav::AbstractControllerExecution::Ptr + newControllerExecution(const std::string& plugin_name, const mbf_abstract_core::AbstractController::Ptr& plugin_ptr); /** * @brief Create a new recovery behavior execution. @@ -132,16 +129,15 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe * @param plugin_ptr Shared pointer to the plugin to use * @return Shared pointer to a new @ref recovery_execution "RecoveryExecution". */ - virtual mbf_abstract_nav::AbstractRecoveryExecution::Ptr newRecoveryExecution( - const std::string &plugin_name, - const mbf_abstract_core::AbstractRecovery::Ptr &plugin_ptr); + virtual mbf_abstract_nav::AbstractRecoveryExecution::Ptr + newRecoveryExecution(const std::string& plugin_name, const mbf_abstract_core::AbstractRecovery::Ptr& plugin_ptr); /** * @brief Loads the plugin associated with the given planner_type parameter. * @param planner_type The type of the planner plugin to load. * @return true, if the local planner plugin was successfully loaded. */ - virtual mbf_abstract_core::AbstractPlanner::Ptr loadPlannerPlugin(const std::string &planner_type); + virtual mbf_abstract_core::AbstractPlanner::Ptr loadPlannerPlugin(const std::string& planner_type); /** * @brief Initializes the controller plugin with its name and pointer to the costmap @@ -149,10 +145,8 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe * @param planner_ptr pointer to the planner object which corresponds to the name param * @return true if init succeeded, false otherwise */ - virtual bool initializePlannerPlugin( - const std::string &name, - const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr - ); + virtual bool initializePlannerPlugin(const std::string& name, + const mbf_abstract_core::AbstractPlanner::Ptr& planner_ptr); /** * @brief Loads the plugin associated with the given controller type parameter @@ -160,7 +154,7 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe * @return A shared pointer to a new loaded controller, if the controller plugin was loaded successfully, * an empty pointer otherwise. */ - virtual mbf_abstract_core::AbstractController::Ptr loadControllerPlugin(const std::string &controller_type); + virtual mbf_abstract_core::AbstractController::Ptr loadControllerPlugin(const std::string& controller_type); /** * @brief Initializes the controller plugin with its name, a pointer to the TransformListener @@ -169,17 +163,15 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe * @param controller_ptr pointer to the controller object which corresponds to the name param * @return true if init succeeded, false otherwise */ - virtual bool initializeControllerPlugin( - const std::string &name, - const mbf_abstract_core::AbstractController::Ptr &controller_ptr - ); + virtual bool initializeControllerPlugin(const std::string& name, + const mbf_abstract_core::AbstractController::Ptr& controller_ptr); /** * @brief Loads a Recovery plugin associated with given recovery type parameter * @param recovery_name The name of the Recovery plugin * @return A shared pointer to a Recovery plugin, if the plugin was loaded successfully, an empty pointer otherwise. */ - virtual mbf_abstract_core::AbstractRecovery::Ptr loadRecoveryPlugin(const std::string &recovery_type); + virtual mbf_abstract_core::AbstractRecovery::Ptr loadRecoveryPlugin(const std::string& recovery_type); /** * @brief Initializes a recovery behavior plugin with its name and pointers to the global and local costmaps @@ -187,9 +179,19 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe * @param behavior_ptr pointer to the recovery behavior object which corresponds to the name param * @return true if init succeeded, false otherwise */ - virtual bool initializeRecoveryPlugin( - const std::string &name, - const mbf_abstract_core::AbstractRecovery::Ptr &behavior_ptr); + virtual bool initializeRecoveryPlugin(const std::string& name, + const mbf_abstract_core::AbstractRecovery::Ptr& behavior_ptr); + + /** + * @brief If mbf_msgs::CheckPose::Request::LOCAL_COSTMAP the local costmap is returned + * if mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP, the global costmap is returned. + * Otherwise, it returns an empty pointer. + * @param costmap_type The type of the costmap to return + * @return A pair: string which is the type of the costmap; and the shared pointer to the requested costmap. + * If costmap is not valid, it returns an empty pointer and an empty string. + */ + std::pair + requestedCostmap(mbf_msgs::CheckPose::Request::_costmap_type costmap_type) const; /** * @brief Callback method for the check_point_cost service @@ -197,8 +199,7 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe * @param response Response object, see the mbf_msgs/CheckPoint service definition file. * @return true, if the service completed successfully, false otherwise */ - bool callServiceCheckPointCost(mbf_msgs::CheckPoint::Request &request, - mbf_msgs::CheckPoint::Response &response); + bool callServiceCheckPointCost(mbf_msgs::CheckPoint::Request& request, mbf_msgs::CheckPoint::Response& response); /** * @brief Callback method for the check_pose_cost service @@ -206,8 +207,7 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe * @param response Response object, see the mbf_msgs/CheckPose service definition file. * @return true, if the service completed successfully, false otherwise */ - bool callServiceCheckPoseCost(mbf_msgs::CheckPose::Request &request, - mbf_msgs::CheckPose::Response &response); + bool callServiceCheckPoseCost(mbf_msgs::CheckPose::Request& request, mbf_msgs::CheckPose::Response& response); /** * @brief Callback method for the check_path_cost service @@ -215,8 +215,7 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe * @param response Response object, see the mbf_msgs/CheckPath service definition file. * @return true, if the service completed successfully, false otherwise */ - bool callServiceCheckPathCost(mbf_msgs::CheckPath::Request &request, - mbf_msgs::CheckPath::Response &response); + bool callServiceCheckPathCost(mbf_msgs::CheckPath::Request& request, mbf_msgs::CheckPath::Response& response); /** * @brief Callback method for the make_plan service @@ -224,14 +223,22 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe * @param response Empty response object. * @return true, if the service completed successfully, false otherwise */ - bool callServiceClearCostmaps(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response); + bool callServiceClearCostmaps(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); + + /** + * @brief Callback method for the find valid pose service + * @param request FindValidPose request object. + * @param response FindValidPose response object. + * @return true, if the service completed successfully, false otherwise + */ + bool callServiceFindValidPose(mbf_msgs::FindValidPose::Request& request, mbf_msgs::FindValidPose::Response& response); /** * @brief Reconfiguration method called by dynamic reconfigure. * @param config Configuration parameters. See the MoveBaseFlexConfig definition. * @param level bit mask, which parameters are set. */ - void reconfigure(mbf_costmap_nav::MoveBaseFlexConfig &config, uint32_t level); + void reconfigure(mbf_costmap_nav::MoveBaseFlexConfig& config, uint32_t level); pluginlib::ClassLoader recovery_plugin_loader_; pluginlib::ClassLoader nav_core_recovery_plugin_loader_; @@ -275,6 +282,11 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe //! Service Server for the clear_costmap service ros::ServiceServer clear_costmaps_srv_; + + //! Service Server for the find_valid_pose service + ros::ServiceServer find_valid_pose_srv_; + + static constexpr double ANGLE_INCREMENT = 5.0 * M_PI / 180.0; // 5 degrees }; } /* namespace mbf_costmap_nav */ diff --git a/mbf_costmap_nav/include/mbf_costmap_nav/footprint_helper.h b/mbf_costmap_nav/include/mbf_costmap_nav/footprint_helper.h index 3a089cb4..a31f9948 100644 --- a/mbf_costmap_nav/include/mbf_costmap_nav/footprint_helper.h +++ b/mbf_costmap_nav/include/mbf_costmap_nav/footprint_helper.h @@ -48,7 +48,7 @@ namespace mbf_costmap_nav struct Cell { - unsigned int x, y; + unsigned int x, y, cost; }; class FootprintHelper @@ -66,6 +66,17 @@ class FootprintHelper const std::vector& footprint_spec, const costmap_2d::Costmap2D&, bool fill); + /** + * @brief Supercover algorithm is a modified Bresenham which prints ALL the points (not only one point per axis) the ideal line contains + * ref: http://eugen.dedu.free.fr/projects/bresenham/ + * @param x0 The x coordinate of the first point + * @param x1 The x coordinate of the second point + * @param y0 The y coordinate of the first point + * @param y1 The y coordinate of the second point + * @param pts Will be filled with the cells that lie on the line in the grid + */ + static void supercover(int x0, int x1, int y0, int y1, std::vector& pts); + /** * @brief Use Bresenham's algorithm to trace a line between two points in a grid * @param x0 The x coordinate of the first point diff --git a/mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search.h b/mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search.h new file mode 100644 index 00000000..fbf3e63d --- /dev/null +++ b/mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search.h @@ -0,0 +1,203 @@ +/* + * Copyright 2023, Rapyuta Robotics Co., Ltd., Renan Salles + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * free_pose_search.h + * + * authors: + * Renan Salles + * + */ + +#ifndef SEARCH_HELPER_H_ +#define SEARCH_HELPER_H_ + +// mbf +#include "mbf_costmap_nav/footprint_helper.h" +#include "mbf_costmap_nav/free_pose_search_viz.h" + +// std +#include +#include +#include + +// ros +#include +#include +#include +#include +#include + +namespace mbf_costmap_nav +{ + +struct SearchConfig +{ + double angle_increment{ 5 * M_PI / 180 }; // 5 degrees + double angle_tolerance{ M_PI_2 }; // 90 degrees + double linear_tolerance{ 1.0 }; // 1 meter + bool use_padded_fp{ true }; // Padded footprint by default + double safety_dist{ 0.1 }; // 10 cm + geometry_msgs::Pose2D goal; +}; + +struct SearchState +{ + unsigned char cost; + std::uint8_t state{ State::UNSET }; + + enum State + { + FREE = 0, + INSCRIBED = 1, + LETHAL = 2, + UNKNOWN = 3, + OUTSIDE = 4, + UNSET = 5 + }; +}; + +struct SearchSolution +{ + geometry_msgs::Pose2D pose; + SearchState search_state; +}; + +/** + * @brief Euclidean Compare parameter for priority queue, defined such that it returns true if its first argument comes + * last its second argument. The Euclidean distance is calculated from the start cell. + * The start cell is given in the constructor, and it is the goal of the search. + */ +class EuclideanCompare +{ +private: + Cell start_; + int start_x_; + int start_y_; + +public: + explicit EuclideanCompare(const Cell& start) : start_(start) + { + start_x_ = static_cast(start.x); + start_y_ = static_cast(start.y); + } + bool operator()(const Cell& a, const Cell& b) + { + int ax = static_cast(a.x); + int bx = static_cast(b.x); + int ay = static_cast(a.y); + int by = static_cast(b.y); + return std::hypot(ax - start_x_, ay - start_y_) > std::hypot(bx - start_x_, by - start_y_); + } +}; + +/** + * @brief It performs a search on the costmap from start to given distance until it finds a cell where we can place the + * robot without colliding with obstacles. + * It is a Breadth-depth search performed in a spiral pattern (neighboring cells added in a priority queue), starting + * from the start (given) cell. And the cells are ordered by euclidean distance from the start cell (by default but one + * can add a new strategy). + * The search stops when it finds a cell where the robot can be placed without colliding with obstacles (footprint + + * padding). + * For each cell, we test multiple angles. + * The search is performed on the costmap given in the constructor. + */ +class FreePoseSearch +{ +protected: + static constexpr std::string_view LOGNAME = "free_pose_search"; + + costmap_2d::Costmap2DROS& costmap_; + SearchConfig config_; + std::function compare_strategy_; + + mutable std::optional viz_; + +public: + FreePoseSearch(costmap_2d::Costmap2DROS& costmap, const SearchConfig& config, + const std::optional>& compare_strategy = std::nullopt, + const std::optional& viz = std::nullopt); + virtual ~FreePoseSearch() = default; + + /** + * @brief It returns the eight neighbors of the given cell + */ + static std::vector getNeighbors(const costmap_2d::Costmap2D& costmap_2d, const Cell& cell); + + /** + * @brief it pads the footprint with the given safety distance + * @param costmap_2d_ros + * @param use_padded_fp If true, it uses the padded footprint, otherwise the unpadded footprint + * @param safety_dist The safety distance to pad the footprint + * @return The padded footprint + */ + static std::vector safetyPadding(costmap_2d::Costmap2DROS& costmap_2d_ros, + const bool use_padded_fp, const double safety_dist); + + /** + * @brief It gets the cost and state of the footprint by checking the max cost of all cells that the footprint covers + * It returns costmap_2d::LETHAL_OBSTACLE if any of the cells is lethal; otherwise, returns costmap_2d::NO_INFORMATION + * if any of the cells is unknown; otherwise the maximum cost of all cells. + * See FindValidPose msg for possible state values. + * @param costmap The costmap2D + * @param footprint The footprint to check + * @param pose_2d The pose to check the footprint + * @return The SearchState of the footprint (FindValidPose.msg state and costmap cost) + */ + static SearchState getFootprintState(const costmap_2d::Costmap2D& costmap_2d, + const std::vector& footprint, + const geometry_msgs::Pose2D& pose_2d); + + /** + * @brief It loops in the given angle increments and checks if the pose of the footprint is valid (collision free) + * It returns the first valid pose found. + * @param costmap_2d The costmap2d + * @param footprint The footprint to check + * @param pose_2d The pose to check the footprint + * @param config The search configuration + * @param viz The visualization object + * @return A search solution for the given pose: best pose, state and cost + */ + static SearchSolution findValidOrientation(const costmap_2d::Costmap2D& costmap_2d, + const std::vector& footprint, + const geometry_msgs::Pose2D& pose_2d, const SearchConfig& config, + std::optional& viz); + + /** + * @brief It performs the search on the costmap, see the class description for more details. + * @param goal The start cell + * @return A search solution for the given pose: best pose, state and cost + */ + virtual SearchSolution search() const; +}; + +} /* namespace mbf_costmap_nav */ +#endif /* SEARCH_HELPER_H_ */ diff --git a/mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search_viz.h b/mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search_viz.h new file mode 100644 index 00000000..3dff641b --- /dev/null +++ b/mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search_viz.h @@ -0,0 +1,100 @@ +/* + * Copyright 2023, Rapyuta Robotics Co., Ltd., Renan Salles + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * free_pose_search_viz.h + * + * authors: + * Renan Salles + * + */ + +#ifndef SEARCH_HELPER_VIZ_H_ +#define SEARCH_HELPER_VIZ_H_ + +// ros +#include +#include +#include +#include +#include + +namespace mbf_costmap_nav +{ +class FreePoseSearchViz +{ +private: + using IdType = visualization_msgs::Marker::_id_type; + static constexpr auto BLOCKED_NS = "blocked_footprints"; + static constexpr auto SOLUTION_NS = "solution"; + + ros::NodeHandle& pnh_; + std::string frame_id_; + + IdType marker_id_{ 0 }; + visualization_msgs::MarkerArray marker_array_; + ros::Publisher marker_pub_; + + void addMarker(const geometry_msgs::Pose2D& pose_2d, const std::vector& footprint, + const std::string& ns, const std_msgs::ColorRGBA& color); + +public: + FreePoseSearchViz(ros::NodeHandle& pnh, const std::string& frame_id); + + /** + * @brief It adds a red marker for a footprint that is blocked by the obstacle in namespace "blocked_footprints" + * @param pose_2d pose of the footprint + * @param frame_id frame of the pose + * @param footprint the footprint + */ + void addBlocked(const geometry_msgs::Pose2D& pose_2d, const std::vector& footprint); + + /** + * @brief It adds a green marker for a footprint that is not blocked by the obstacle in namespace "solution" + * @param pose_2d pose of the footprint + * @param frame_id frame of the pose + * @param footprint the footprint + */ + void addSolution(const geometry_msgs::Pose2D& pose_2d, const std::vector& footprint); + + /** + * @brief It clear the previous viz and publishes the markers + */ + void publish(); + + /** + * @brief It clears the previous viz + */ + void deleteMarkers(); +}; + +} /* namespace mbf_costmap_nav */ +#endif /* SEARCH_HELPER_VIZ_H_ */ diff --git a/mbf_costmap_nav/package.xml b/mbf_costmap_nav/package.xml index cbf3bc32..35689e0e 100644 --- a/mbf_costmap_nav/package.xml +++ b/mbf_costmap_nav/package.xml @@ -35,6 +35,9 @@ move_base move_base_msgs + gtest + map_server + diff --git a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp index e003641a..29cde0fd 100644 --- a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp +++ b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp @@ -51,6 +51,8 @@ #include "mbf_costmap_nav/footprint_helper.h" #include "mbf_costmap_nav/costmap_navigation_server.h" +#include "mbf_costmap_nav/free_pose_search.h" +#include "mbf_costmap_nav/free_pose_search_viz.h" namespace mbf_costmap_nav { @@ -153,27 +155,29 @@ StringToMap loadStringToMaps(const std::string& resource, const ros::NodeHandle& return StringToMap(); } -CostmapNavigationServer::CostmapNavigationServer(const TFPtr &tf_listener_ptr) : - AbstractNavigationServer(tf_listener_ptr), - recovery_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapRecovery"), - nav_core_recovery_plugin_loader_("nav_core", "nav_core::RecoveryBehavior"), - controller_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapController"), - nav_core_controller_plugin_loader_("nav_core", "nav_core::BaseLocalPlanner"), - planner_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapPlanner"), - nav_core_planner_plugin_loader_("nav_core", "nav_core::BaseGlobalPlanner"), - global_costmap_ptr_(new CostmapWrapper("global_costmap", tf_listener_ptr_)), - local_costmap_ptr_(new CostmapWrapper("local_costmap", tf_listener_ptr_)), - setup_reconfigure_(false) +CostmapNavigationServer::CostmapNavigationServer(const TFPtr& tf_listener_ptr) + : AbstractNavigationServer(tf_listener_ptr) + , recovery_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapRecovery") + , nav_core_recovery_plugin_loader_("nav_core", "nav_core::RecoveryBehavior") + , controller_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapController") + , nav_core_controller_plugin_loader_("nav_core", "nav_core::BaseLocalPlanner") + , planner_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapPlanner") + , nav_core_planner_plugin_loader_("nav_core", "nav_core::BaseGlobalPlanner") + , global_costmap_ptr_(new CostmapWrapper("global_costmap", tf_listener_ptr_)) + , local_costmap_ptr_(new CostmapWrapper("local_costmap", tf_listener_ptr_)) + , setup_reconfigure_(false) { // advertise services and current goal topic - check_point_cost_srv_ = private_nh_.advertiseService("check_point_cost", - &CostmapNavigationServer::callServiceCheckPointCost, this); - check_pose_cost_srv_ = private_nh_.advertiseService("check_pose_cost", - &CostmapNavigationServer::callServiceCheckPoseCost, this); - check_path_cost_srv_ = private_nh_.advertiseService("check_path_cost", - &CostmapNavigationServer::callServiceCheckPathCost, this); - clear_costmaps_srv_ = private_nh_.advertiseService("clear_costmaps", - &CostmapNavigationServer::callServiceClearCostmaps, this); + check_point_cost_srv_ = + private_nh_.advertiseService("check_point_cost", &CostmapNavigationServer::callServiceCheckPointCost, this); + check_pose_cost_srv_ = + private_nh_.advertiseService("check_pose_cost", &CostmapNavigationServer::callServiceCheckPoseCost, this); + check_path_cost_srv_ = + private_nh_.advertiseService("check_path_cost", &CostmapNavigationServer::callServiceCheckPathCost, this); + clear_costmaps_srv_ = + private_nh_.advertiseService("clear_costmaps", &CostmapNavigationServer::callServiceClearCostmaps, this); + find_valid_pose_srv_ = + private_nh_.advertiseService("find_valid_pose", &CostmapNavigationServer::callServiceFindValidPose, this); // dynamic reconfigure server for mbf_costmap_nav configuration; also include abstract server parameters dsrv_costmap_ = boost::make_shared >(private_nh_); @@ -215,37 +219,34 @@ const Value& findWithDefault(const boost::unordered_map& map, const } mbf_abstract_nav::AbstractPlannerExecution::Ptr CostmapNavigationServer::newPlannerExecution( - const std::string &plugin_name, - const mbf_abstract_core::AbstractPlanner::Ptr &plugin_ptr) + const std::string& plugin_name, const mbf_abstract_core::AbstractPlanner::Ptr& plugin_ptr) { const CostmapWrapper::Ptr& costmap_ptr = findWithDefault(planner_name_to_costmap_ptr_, plugin_name, global_costmap_ptr_); return boost::make_shared( - plugin_name, boost::static_pointer_cast(plugin_ptr), robot_info_, - costmap_ptr, last_config_); + plugin_name, boost::static_pointer_cast(plugin_ptr), robot_info_, costmap_ptr, + last_config_); } mbf_abstract_nav::AbstractControllerExecution::Ptr CostmapNavigationServer::newControllerExecution( - const std::string &plugin_name, - const mbf_abstract_core::AbstractController::Ptr &plugin_ptr) + const std::string& plugin_name, const mbf_abstract_core::AbstractController::Ptr& plugin_ptr) { const CostmapWrapper::Ptr& costmap_ptr = findWithDefault(controller_name_to_costmap_ptr_, plugin_name, local_costmap_ptr_); return boost::make_shared( - plugin_name, boost::static_pointer_cast(plugin_ptr), robot_info_, - vel_pub_, goal_pub_, costmap_ptr, last_config_); + plugin_name, boost::static_pointer_cast(plugin_ptr), robot_info_, vel_pub_, + goal_pub_, costmap_ptr, last_config_); } mbf_abstract_nav::AbstractRecoveryExecution::Ptr CostmapNavigationServer::newRecoveryExecution( - const std::string &plugin_name, - const mbf_abstract_core::AbstractRecovery::Ptr &plugin_ptr) + const std::string& plugin_name, const mbf_abstract_core::AbstractRecovery::Ptr& plugin_ptr) { return boost::make_shared( plugin_name, boost::static_pointer_cast(plugin_ptr), robot_info_, global_costmap_ptr_, local_costmap_ptr_, last_config_); } -mbf_abstract_core::AbstractPlanner::Ptr CostmapNavigationServer::loadPlannerPlugin(const std::string &planner_type) +mbf_abstract_core::AbstractPlanner::Ptr CostmapNavigationServer::loadPlannerPlugin(const std::string& planner_type) { mbf_abstract_core::AbstractPlanner::Ptr planner_ptr; try @@ -255,35 +256,35 @@ mbf_abstract_core::AbstractPlanner::Ptr CostmapNavigationServer::loadPlannerPlug std::string planner_name = planner_plugin_loader_.getName(planner_type); ROS_DEBUG_STREAM("mbf_costmap_core-based planner plugin " << planner_name << " loaded."); } - catch (const pluginlib::PluginlibException &ex_mbf_core) + catch (const pluginlib::PluginlibException& ex_mbf_core) { ROS_DEBUG_STREAM("Failed to load the " << planner_type << " planner as a mbf_costmap_core-based plugin." - << " Try to load as a nav_core-based plugin. " << ex_mbf_core.what()); + << " Try to load as a nav_core-based plugin. " << ex_mbf_core.what()); try { // For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper - boost::shared_ptr nav_core_planner_ptr = nav_core_planner_plugin_loader_.createInstance(planner_type); + boost::shared_ptr nav_core_planner_ptr = + nav_core_planner_plugin_loader_.createInstance(planner_type); planner_ptr = boost::make_shared(nav_core_planner_ptr); std::string planner_name = nav_core_planner_plugin_loader_.getName(planner_type); ROS_DEBUG_STREAM("nav_core-based planner plugin " << planner_name << " loaded"); } - catch (const pluginlib::PluginlibException &ex_nav_core) + catch (const pluginlib::PluginlibException& ex_nav_core) { ROS_FATAL_STREAM("Failed to load the " << planner_type << " planner, are you sure it's properly registered" - << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what()); + << " and that the containing library is built? " << ex_mbf_core.what() + << " " << ex_nav_core.what()); } } return planner_ptr; } -bool CostmapNavigationServer::initializePlannerPlugin( - const std::string &name, - const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr -) +bool CostmapNavigationServer::initializePlannerPlugin(const std::string& name, + const mbf_abstract_core::AbstractPlanner::Ptr& planner_ptr) { - mbf_costmap_core::CostmapPlanner::Ptr costmap_planner_ptr - = boost::static_pointer_cast(planner_ptr); + mbf_costmap_core::CostmapPlanner::Ptr costmap_planner_ptr = + boost::static_pointer_cast(planner_ptr); ROS_DEBUG_STREAM("Initialize planner \"" << name << "\"."); const CostmapWrapper::Ptr& costmap_ptr = findWithDefault(planner_name_to_costmap_ptr_, name, global_costmap_ptr_); @@ -298,8 +299,8 @@ bool CostmapNavigationServer::initializePlannerPlugin( return true; } - -mbf_abstract_core::AbstractController::Ptr CostmapNavigationServer::loadControllerPlugin(const std::string &controller_type) +mbf_abstract_core::AbstractController::Ptr +CostmapNavigationServer::loadControllerPlugin(const std::string& controller_type) { mbf_abstract_core::AbstractController::Ptr controller_ptr; try @@ -308,31 +309,32 @@ mbf_abstract_core::AbstractController::Ptr CostmapNavigationServer::loadControll std::string controller_name = controller_plugin_loader_.getName(controller_type); ROS_DEBUG_STREAM("mbf_costmap_core-based controller plugin " << controller_name << " loaded."); } - catch (const pluginlib::PluginlibException &ex_mbf_core) + catch (const pluginlib::PluginlibException& ex_mbf_core) { ROS_DEBUG_STREAM("Failed to load the " << controller_type << " controller as a mbf_costmap_core-based plugin;" - << " we will retry to load as a nav_core-based plugin. " << ex_mbf_core.what()); + << " we will retry to load as a nav_core-based plugin. " + << ex_mbf_core.what()); try { // For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper - boost::shared_ptr nav_core_controller_ptr - = nav_core_controller_plugin_loader_.createInstance(controller_type); + boost::shared_ptr nav_core_controller_ptr = + nav_core_controller_plugin_loader_.createInstance(controller_type); controller_ptr = boost::make_shared(nav_core_controller_ptr); std::string controller_name = nav_core_controller_plugin_loader_.getName(controller_type); ROS_DEBUG_STREAM("nav_core-based controller plugin " << controller_name << " loaded."); } - catch (const pluginlib::PluginlibException &ex_nav_core) + catch (const pluginlib::PluginlibException& ex_nav_core) { ROS_FATAL_STREAM("Failed to load the " << controller_type << " controller, are you sure it's properly registered" - << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what()); + << " and that the containing library is built? " << ex_mbf_core.what() + << " " << ex_nav_core.what()); } } return controller_ptr; } bool CostmapNavigationServer::initializeControllerPlugin( - const std::string &name, - const mbf_abstract_core::AbstractController::Ptr &controller_ptr) + const std::string& name, const mbf_abstract_core::AbstractController::Ptr& controller_ptr) { ROS_DEBUG_STREAM("Initialize controller \"" << name << "\"."); @@ -350,15 +352,14 @@ bool CostmapNavigationServer::initializeControllerPlugin( return false; } - mbf_costmap_core::CostmapController::Ptr costmap_controller_ptr - = boost::static_pointer_cast(controller_ptr); + mbf_costmap_core::CostmapController::Ptr costmap_controller_ptr = + boost::static_pointer_cast(controller_ptr); costmap_controller_ptr->initialize(name, tf_listener_ptr_.get(), costmap_ptr.get()); ROS_DEBUG_STREAM("Controller plugin \"" << name << "\" initialized."); return true; } -mbf_abstract_core::AbstractRecovery::Ptr CostmapNavigationServer::loadRecoveryPlugin( - const std::string &recovery_type) +mbf_abstract_core::AbstractRecovery::Ptr CostmapNavigationServer::loadRecoveryPlugin(const std::string& recovery_type) { mbf_abstract_core::AbstractRecovery::Ptr recovery_ptr; @@ -369,10 +370,10 @@ mbf_abstract_core::AbstractRecovery::Ptr CostmapNavigationServer::loadRecoveryPl std::string recovery_name = recovery_plugin_loader_.getName(recovery_type); ROS_DEBUG_STREAM("mbf_costmap_core-based recovery behavior plugin " << recovery_name << " loaded."); } - catch (pluginlib::PluginlibException &ex_mbf_core) + catch (pluginlib::PluginlibException& ex_mbf_core) { ROS_DEBUG_STREAM("Failed to load the " << recovery_type << " recovery behavior as a mbf_costmap_core-based plugin;" - << " Retry to load as a nav_core-based plugin. " << ex_mbf_core.what()); + << " Retry to load as a nav_core-based plugin. " << ex_mbf_core.what()); try { // For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper @@ -382,21 +383,21 @@ mbf_abstract_core::AbstractRecovery::Ptr CostmapNavigationServer::loadRecoveryPl recovery_ptr = boost::make_shared(nav_core_recovery_ptr); std::string recovery_name = recovery_plugin_loader_.getName(recovery_type); ROS_DEBUG_STREAM("nav_core-based recovery behavior plugin " << recovery_name << " loaded."); - } - catch (const pluginlib::PluginlibException &ex_nav_core) + catch (const pluginlib::PluginlibException& ex_nav_core) { - ROS_FATAL_STREAM("Failed to load the " << recovery_type << " recovery behavior, are you sure it's properly registered" - << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what()); + ROS_FATAL_STREAM("Failed to load the " + << recovery_type << " recovery behavior, are you sure it's properly registered" + << " and that the containing library is built? " << ex_mbf_core.what() << " " + << ex_nav_core.what()); } } return recovery_ptr; } -bool CostmapNavigationServer::initializeRecoveryPlugin( - const std::string &name, - const mbf_abstract_core::AbstractRecovery::Ptr &behavior_ptr) +bool CostmapNavigationServer::initializeRecoveryPlugin(const std::string& name, + const mbf_abstract_core::AbstractRecovery::Ptr& behavior_ptr) { ROS_DEBUG_STREAM("Initialize recovery behavior \"" << name << "\"."); @@ -425,7 +426,6 @@ bool CostmapNavigationServer::initializeRecoveryPlugin( return true; } - void CostmapNavigationServer::stop() { AbstractNavigationServer::stop(); @@ -434,7 +434,7 @@ void CostmapNavigationServer::stop() global_costmap_ptr_->stop(); } -void CostmapNavigationServer::reconfigure(mbf_costmap_nav::MoveBaseFlexConfig &config, uint32_t level) +void CostmapNavigationServer::reconfigure(mbf_costmap_nav::MoveBaseFlexConfig& config, uint32_t level) { // Make sure we have the original configuration the first time we're called, so we can restore it if needed if (!setup_reconfigure_) @@ -472,8 +472,8 @@ void CostmapNavigationServer::reconfigure(mbf_costmap_nav::MoveBaseFlexConfig &c last_config_ = config; } -bool CostmapNavigationServer::callServiceCheckPointCost(mbf_msgs::CheckPoint::Request &request, - mbf_msgs::CheckPoint::Response &response) +bool CostmapNavigationServer::callServiceCheckPointCost(mbf_msgs::CheckPoint::Request& request, + mbf_msgs::CheckPoint::Response& response) { // selecting the requested costmap CostmapWrapper::Ptr costmap; @@ -490,8 +490,8 @@ bool CostmapNavigationServer::callServiceCheckPointCost(mbf_msgs::CheckPoint::Re break; default: ROS_ERROR_STREAM("No valid costmap provided; options are " - << mbf_msgs::CheckPoint::Request::LOCAL_COSTMAP << ": local costmap, " - << mbf_msgs::CheckPoint::Request::GLOBAL_COSTMAP << ": global costmap"); + << mbf_msgs::CheckPoint::Request::LOCAL_COSTMAP << ": local costmap, " + << mbf_msgs::CheckPoint::Request::GLOBAL_COSTMAP << ": global costmap"); return false; } @@ -549,8 +549,8 @@ bool CostmapNavigationServer::callServiceCheckPointCost(mbf_msgs::CheckPoint::Re return true; } -bool CostmapNavigationServer::callServiceCheckPoseCost(mbf_msgs::CheckPose::Request &request, - mbf_msgs::CheckPose::Response &response) +bool CostmapNavigationServer::callServiceCheckPoseCost(mbf_msgs::CheckPose::Request& request, + mbf_msgs::CheckPose::Response& response) { // selecting the requested costmap CostmapWrapper::Ptr costmap; @@ -607,7 +607,7 @@ bool CostmapNavigationServer::callServiceCheckPoseCost(mbf_msgs::CheckPose::Requ // use footprint helper to get all the cells totally or partially within footprint polygon std::vector footprint_cells = - FootprintHelper::getFootprintCells(x, y, yaw, footprint, *costmap->getCostmap(), true); + FootprintHelper::getFootprintCells(x, y, yaw, footprint, *costmap->getCostmap(), true); response.state = mbf_msgs::CheckPose::Response::FREE; if (footprint_cells.empty()) { @@ -673,27 +673,13 @@ bool CostmapNavigationServer::callServiceCheckPoseCost(mbf_msgs::CheckPose::Requ return true; } -bool CostmapNavigationServer::callServiceCheckPathCost(mbf_msgs::CheckPath::Request &request, - mbf_msgs::CheckPath::Response &response) +bool CostmapNavigationServer::callServiceCheckPathCost(mbf_msgs::CheckPath::Request& request, + mbf_msgs::CheckPath::Response& response) { - // selecting the requested costmap - CostmapWrapper::Ptr costmap; - std::string costmap_name; - switch (request.costmap) + const auto& [costmap_name, costmap] = requestedCostmap(request.costmap); + if (!costmap) { - case mbf_msgs::CheckPath::Request::LOCAL_COSTMAP: - costmap = local_costmap_ptr_; - costmap_name = "local costmap"; - break; - case mbf_msgs::CheckPath::Request::GLOBAL_COSTMAP: - costmap = global_costmap_ptr_; - costmap_name = "global costmap"; - break; - default: - ROS_ERROR_STREAM("No valid costmap provided; options are " - << mbf_msgs::CheckPath::Request::LOCAL_COSTMAP << ": local costmap, " - << mbf_msgs::CheckPath::Request::GLOBAL_COSTMAP << ": global costmap"); - return false; + return false; } // ensure costmap is active so cost reflects the latest sensor readings @@ -773,7 +759,8 @@ bool CostmapNavigationServer::callServiceCheckPathCost(mbf_msgs::CheckPath::Requ response.state = std::max(response.state, static_cast(mbf_msgs::CheckPath::Response::INSCRIBED)); response.cost += cost * (request.inscrib_cost_mult ? request.inscrib_cost_mult : 1.0); break; - default:response.cost += cost; + default: + response.cost += cost; break; } } @@ -786,23 +773,27 @@ bool CostmapNavigationServer::callServiceCheckPathCost(mbf_msgs::CheckPath::Requ { case mbf_msgs::CheckPath::Response::OUTSIDE: ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes outside the map " - << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")"); + << "(cost = " << response.cost << "; safety distance = " << request.safety_dist + << ")"); break; case mbf_msgs::CheckPath::Response::UNKNOWN: ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes in unknown space! " - << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")"); + << "(cost = " << response.cost << "; safety distance = " << request.safety_dist + << ")"); break; case mbf_msgs::CheckPath::Response::LETHAL: ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes in collision! " - << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")"); + << "(cost = " << response.cost << "; safety distance = " << request.safety_dist + << ")"); break; case mbf_msgs::CheckPath::Response::INSCRIBED: ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes near an obstacle " - << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")"); + << "(cost = " << response.cost << "; safety distance = " << request.safety_dist + << ")"); break; case mbf_msgs::CheckPath::Response::FREE: - ROS_DEBUG_STREAM("Path is entirely free (maximum cost = " - << response.cost << "; safety distance = " << request.safety_dist << ")"); + ROS_DEBUG_STREAM("Path is entirely free (maximum cost = " << response.cost << "; safety distance = " + << request.safety_dist << ")"); break; } @@ -816,8 +807,8 @@ bool CostmapNavigationServer::callServiceCheckPathCost(mbf_msgs::CheckPath::Requ return true; } -bool CostmapNavigationServer::callServiceClearCostmaps(std_srvs::Empty::Request &request, - std_srvs::Empty::Response &response) +bool CostmapNavigationServer::callServiceClearCostmaps(std_srvs::Empty::Request& request, + std_srvs::Empty::Response& response) { // clear both costmaps local_costmap_ptr_->clear(); @@ -825,4 +816,81 @@ bool CostmapNavigationServer::callServiceClearCostmaps(std_srvs::Empty::Request return true; } +std::pair CostmapNavigationServer::requestedCostmap(std::uint8_t costmap_type) const +{ + // selecting the requested costmap + CostmapWrapper::Ptr costmap; + switch (costmap_type) + { + case mbf_msgs::CheckPose::Request::LOCAL_COSTMAP: + return { "local_costmap", local_costmap_ptr_ }; + break; + case mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP: + return { "global_costmap", global_costmap_ptr_ }; + break; + default: + ROS_ERROR_STREAM("No valid costmap provided; options are " + << mbf_msgs::CheckPose::Request::LOCAL_COSTMAP << ": local costmap, " + << mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP << ": global costmap"); + return { "", costmap }; + } +} + +bool CostmapNavigationServer::callServiceFindValidPose(mbf_msgs::FindValidPose::Request& request, + mbf_msgs::FindValidPose::Response& response) +{ + const auto& [costmap_name, costmap] = requestedCostmap(request.costmap); + if (!costmap) + { + return false; + } + + // get target pose or current robot pose as x, y, yaw coordinates + const std::string costmap_frame = costmap->getGlobalFrameID(); + + geometry_msgs::PoseStamped pose; + if (request.current_pose) + { + if (!mbf_utility::getRobotPose(*tf_listener_ptr_, robot_frame_, costmap_frame, ros::Duration(0.5), pose)) + { + ROS_ERROR_STREAM("Get robot pose on " << costmap_name << " frame '" << costmap_frame << "' failed"); + return false; + } + } + else + { + if (!mbf_utility::transformPose(*tf_listener_ptr_, costmap_frame, ros::Duration(0.5), request.pose, pose)) + { + ROS_ERROR_STREAM("Transform target pose to " << costmap_name << " frame '" << costmap_frame << "' failed"); + return false; + } + } + + geometry_msgs::Pose2D goal; + goal.x = request.pose.pose.position.x; + goal.y = request.pose.pose.position.y; + goal.theta = tf::getYaw(request.pose.pose.orientation); + + ros::NodeHandle private_nh("~"); + // using 5 degrees as increment + const SearchConfig config{ ANGLE_INCREMENT, request.angle_tolerance, + request.dist_tolerance, static_cast(request.use_padded_fp), + request.safety_dist, goal }; + FreePoseSearchViz viz(private_nh, costmap_frame); + FreePoseSearch free_pose_search(*costmap.get(), config, std::nullopt, viz); + + // search for a valid pose + const auto sol = free_pose_search.search(); + + response.pose.pose.position.x = sol.pose.x; + response.pose.pose.position.y = sol.pose.y; + response.pose.pose.position.z = 0; + response.pose.pose.orientation = tf::createQuaternionMsgFromYaw(sol.pose.theta); + response.pose.header.frame_id = costmap_frame; + response.pose.header.stamp = ros::Time::now(); + response.state = sol.search_state.state; + response.cost = sol.search_state.cost; + return true; +} + } /* namespace mbf_costmap_nav */ diff --git a/mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp b/mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp index a932e361..88700d25 100644 --- a/mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp +++ b/mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp @@ -111,6 +111,102 @@ void FootprintHelper::getLineCells(int x0, int x1, int y0, int y1, std::vector& pts) +{ + { + auto add_point = [&pts](int y, int x) { + Cell pt; + pt.x = x; + pt.y = y; + pts.push_back(pt); + }; + + int i; // loop counter + int ystep, xstep; // the step on y and x axis + int error; // the error accumulated during the increment + int errorprev; // *vision the previous value of the error variable + int y = y0, x = x0; // the line points + int ddy, ddx; // compulsory variables: the double values of dy and dx + int dx = x1 - x0; + int dy = y1 - y0; + add_point(y0, x0); // first point + + // NB the last point can't be here, because of its previous point (which has to be verified) + if (dy < 0) + { + ystep = -1; + dy = -dy; + } + else + { + ystep = 1; + } + if (dx < 0) + { + xstep = -1; + dx = -dx; + } + else + { + xstep = 1; + } + + ddy = 2 * dy; // work with double values for full precision + ddx = 2 * dx; + if (ddx >= ddy) + { // first octant (0 <= slope <= 1) + // compulsory initialization (even for errorprev, needed when dx==dy) + errorprev = error = dx; // start in the middle of the square + for (i = 0; i < dx; i++) + { // do not use the first point (already done) + x += xstep; + error += ddy; + if (error > ddx) + { // increment y if AFTER the middle ( > ) + y += ystep; + error -= ddx; + // three cases (octant == right->right-top for directions below): + if (error + errorprev < ddx) // bottom square also + add_point(y - ystep, x); + else if (error + errorprev > ddx) // left square also + add_point(y, x - xstep); + else + { // corner: bottom and left squares also + add_point(y - ystep, x); + add_point(y, x - xstep); + } + } + add_point(y, x); + errorprev = error; + } + } + else + { // the same as above + errorprev = error = dy; + for (i = 0; i < dy; i++) + { + y += ystep; + error += ddx; + if (error > ddy) + { + x += xstep; + error -= ddy; + if (error + errorprev < ddy) + add_point(y, x - xstep); + else if (error + errorprev > ddy) + add_point(y - ystep, x); + else + { + add_point(y, x - xstep); + add_point(y - ystep, x); + } + } + add_point(y, x); + errorprev = error; + } + } + } +} void FootprintHelper::getFillCells(std::vector& footprint){ //quick bubble sort to sort pts by x diff --git a/mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search.cpp b/mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search.cpp new file mode 100644 index 00000000..7068a049 --- /dev/null +++ b/mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search.cpp @@ -0,0 +1,359 @@ +/* + * Copyright 2023, Rapyuta Robotics Co., Ltd., Renan Salles + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * free_pose_search.cpp + * + * authors: + * Renan Salles + * + */ + +// mbf_costmap_nav +#include "mbf_costmap_nav/free_pose_search.h" + +// std +#include +#include + +namespace mbf_costmap_nav +{ + +FreePoseSearch::FreePoseSearch(costmap_2d::Costmap2DROS& costmap, const SearchConfig& config, + const std::optional>& compare_strategy, + const std::optional& viz) + : costmap_(costmap), config_(config), viz_(viz) +{ + Cell start; + costmap_.getCostmap()->worldToMap(config_.goal.x, config_.goal.y, start.x, start.y); + ROS_DEBUG_STREAM("Start cell: " << start.x << ", " << start.y); + EuclideanCompare euclidean_compare(start); + compare_strategy_ = std::move(compare_strategy.value_or(euclidean_compare)); +} + +std::vector FreePoseSearch::getNeighbors(const costmap_2d::Costmap2D& costmap_2d, const Cell& cell) +{ + std::vector neighbors; + neighbors.reserve(8); + for (int dx = -1; dx <= 1; ++dx) + { + for (int dy = -1; dy <= 1; ++dy) + { + if (dx == 0 && dy == 0) + { + continue; + } + unsigned int x = cell.x + dx; + unsigned int y = cell.y + dy; + if (x < costmap_2d.getSizeInCellsX() && y < costmap_2d.getSizeInCellsY() && x >= 0 && y >= 0) + { + neighbors.push_back(Cell{ x, y, costmap_2d.getCost(x, y) }); + } + } + } + return neighbors; +} + +std::vector FreePoseSearch::safetyPadding(costmap_2d::Costmap2DROS& costmap_2d_ros, + const bool use_padded_fp, const double safety_dist) + +{ + std::vector footprint = + use_padded_fp ? costmap_2d_ros.getRobotFootprint() : costmap_2d_ros.getUnpaddedRobotFootprint(); + costmap_2d::padFootprint(footprint, safety_dist); + return footprint; +} + +SearchState FreePoseSearch::getFootprintState(const costmap_2d::Costmap2D& costmap_2d, + const std::vector& footprint, + const geometry_msgs::Pose2D& pose_2d) +{ + const auto cells_to_check = + FootprintHelper::getFootprintCells(pose_2d.x, pose_2d.y, pose_2d.theta, footprint, costmap_2d, true); + if (cells_to_check.empty()) + { + return { costmap_2d::NO_INFORMATION, SearchState::OUTSIDE }; + } + + // create a map of cells to avoid duplicates + std::unordered_map cells_to_check_map; + for (const auto& cell : cells_to_check) + { + cells_to_check_map[costmap_2d.getIndex(cell.x, cell.y)] = cell; + } + + unsigned char max_cost = 0; + for (const auto& [_, cell] : cells_to_check_map) + { + unsigned char cost = costmap_2d.getCost(cell.x, cell.y); + switch (cost) + { + case costmap_2d::LETHAL_OBSTACLE: + return { costmap_2d::LETHAL_OBSTACLE, SearchState::LETHAL }; + break; + default: + max_cost = std::max(max_cost, cost); + break; + } + } + uint8_t state = max_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ? SearchState::INSCRIBED : + max_cost == costmap_2d::NO_INFORMATION ? SearchState::UNKNOWN : + SearchState::FREE; + return { max_cost, state }; +} + +SearchSolution FreePoseSearch::findValidOrientation(const costmap_2d::Costmap2D& costmap_2d, + const std::vector& footprint, + const geometry_msgs::Pose2D& pose_2d, const SearchConfig& config, + std::optional& viz) +{ + bool outside_or_unknown = false; + SearchSolution sol; + SearchSolution outside_or_unknown_sol; + sol.pose = pose_2d; + + for (double dyaw = 0; dyaw <= config.angle_tolerance; dyaw += config.angle_increment) + { + const std::vector thetas = dyaw == 0 ? std::vector{ pose_2d.theta } : + std::vector{ pose_2d.theta + dyaw, pose_2d.theta - dyaw }; + for (const auto& theta : thetas) + { + sol.pose.theta = theta; + SearchState search_state = getFootprintState(costmap_2d, footprint, sol.pose); + + switch (search_state.state) + { + case SearchState::FREE: + case SearchState::INSCRIBED: + if (viz) + { + viz->addSolution(sol.pose, footprint); + } + ROS_DEBUG_STREAM_NAMED(LOGNAME.data(), "Found solution at: " << sol.pose.x << ", " << sol.pose.y << ", " + << sol.pose.theta + << " with state: " << search_state.state); + return { sol.pose, search_state }; + case SearchState::UNKNOWN: + case SearchState::OUTSIDE: + if (!outside_or_unknown) + { + // we save the first unknown/outside pose, but we continue searching for a free pose + outside_or_unknown_sol = { sol.pose, search_state }; + outside_or_unknown = true; + } + break; + case SearchState::LETHAL: + // if we didn't find a free pose or unknown/outside, we return lethal + if (!outside_or_unknown) + { + sol.search_state = search_state; + } + if (viz) + { + viz->addBlocked(sol.pose, footprint); + } + break; + default: + ROS_ERROR_STREAM("Unknown state: " << search_state.state); + break; + } + } + } + + if (outside_or_unknown) + { + ROS_DEBUG_STREAM_COND_NAMED(outside_or_unknown_sol.search_state.state == SearchState::UNKNOWN, LOGNAME.data(), + "Solution is in unknown space for pose x-y-theta (" + << pose_2d.x << ", " << pose_2d.y << ", " << pose_2d.theta << ") with tolerance " + << config.angle_tolerance << " and increment " << config.angle_increment); + + ROS_DEBUG_STREAM_COND_NAMED(outside_or_unknown_sol.search_state.state == SearchState::OUTSIDE, LOGNAME.data(), + "Solution is partially outside the map for pose x-y-theta (" + << pose_2d.x << ", " << pose_2d.y << ", " << pose_2d.theta << ") with tolerance " + << config.angle_tolerance << " and increment " << config.angle_increment); + return outside_or_unknown_sol; + } + + ROS_DEBUG_STREAM_COND_NAMED(sol.search_state.state == SearchState::LETHAL, LOGNAME.data(), + "No valid orientation found for pose x-y-theta (" + << pose_2d.x << ", " << pose_2d.y << ", " << pose_2d.theta << ") with tolerance " + << config.angle_tolerance << " and increment " << config.angle_increment); + + return sol; +} + +SearchSolution FreePoseSearch::search() const +{ + const auto costmap2d = costmap_.getCostmap(); + + // lock costmap so content doesn't change while adding cell costs + boost::unique_lock lock(*(costmap2d->getMutex())); + + std::unordered_set in_queue_or_visited; + + const std::vector footprint = + safetyPadding(costmap_, config_.use_padded_fp, config_.safety_dist); + + SearchSolution sol; + sol.pose.theta = config_.goal.theta; + bool outside_or_unknown{ false }; + + // initializing queue with the goal cell + Cell start; + if (costmap2d->worldToMap(config_.goal.x, config_.goal.y, start.x, start.y)) + { + start.cost = costmap2d->getCost(start.x, start.y); + } + else + { + // enforce bounds if goal is outside the map + // note that distance tolerance is wrt this new start cell + int cell_x, cell_y; + costmap2d->worldToMapEnforceBounds(config_.goal.x, config_.goal.y, cell_x, cell_y); + start.x = static_cast(cell_x); + start.y = static_cast(cell_y); + start.cost = costmap2d->getCost(start.x, start.y); + } + in_queue_or_visited.insert(costmap2d->getIndex(start.x, start.y)); + + std::priority_queue, decltype(compare_strategy_)> queue(compare_strategy_); + queue.push(start); + + // restart markers + if (viz_) + { + viz_->deleteMarkers(); + } + + while (!queue.empty()) + { + Cell cell = queue.top(); + queue.pop(); + + costmap2d->mapToWorld(cell.x, cell.y, sol.pose.x, sol.pose.y); + ROS_DEBUG_STREAM_NAMED(LOGNAME.data(), "Checking Cell: (" << cell.x << ", " << cell.y << ") with pose: (" + << sol.pose.x << ", " << sol.pose.y << ", " + << sol.pose.theta << ")"); + + // Note: if the center of the robot is in costmap_2d::NO_INFORMATION, we don't accept it as a solution + if (cell.cost != costmap_2d::LETHAL_OBSTACLE && cell.cost != costmap_2d::INSCRIBED_INFLATED_OBSTACLE && + cell.cost != costmap_2d::NO_INFORMATION) + { + const auto tested_sol = findValidOrientation(*costmap2d, footprint, sol.pose, config_, viz_); + // if footprint is free or inscribed, we return the solution + if (tested_sol.search_state.state == SearchState::FREE || tested_sol.search_state.state == SearchState::INSCRIBED) + { + ROS_DEBUG_STREAM_NAMED(LOGNAME.data(), "Found solution pose: " << tested_sol.pose.x << ", " << tested_sol.pose.y + << ", " << tested_sol.pose.theta); + if (viz_) + { + viz_->publish(); + } + return tested_sol; + } + + // if the state is outside or unknown, we save the first one we find + if ((tested_sol.search_state.state == SearchState::OUTSIDE || + tested_sol.search_state.state == SearchState::UNKNOWN) && + !outside_or_unknown) + { + ROS_DEBUG_STREAM_NAMED(LOGNAME.data(), "Found unknown/outside pose: " << tested_sol.pose.x << ", " + << tested_sol.pose.y << ", " + << tested_sol.pose.theta); + sol = tested_sol; + outside_or_unknown = true; + } + + ROS_DEBUG_STREAM_COND_NAMED(tested_sol.search_state.state == SearchState::LETHAL, LOGNAME.data(), + "Footprint in cell " << cell.x << ", " << cell.y << "; pose: (" << sol.pose.x << ", " + << sol.pose.y << ", " << sol.pose.theta + << ") is in lethal obstacle or unknown; skipping"); + } + + else + { + ROS_DEBUG_STREAM_NAMED(LOGNAME.data(), "Cell: (" << cell.x << ", " << cell.y << ") with pose: (" << sol.pose.x + << ", " << sol.pose.y << ", " << sol.pose.theta + << ") is in lethal obstacle or in unknown space; skipping"); + } + + // adding neighbors to queue + const std::vector neighbors = getNeighbors(*costmap2d, cell); + for (const Cell& neighbor : neighbors) + { + int cell_index = costmap2d->getIndex(neighbor.x, neighbor.y); + if (in_queue_or_visited.find(cell_index) != in_queue_or_visited.end()) + { + ROS_DEBUG_STREAM("Cell " << neighbor.x << ", " << neighbor.y << " already in queue or visited; skipping"); + continue; + } + + // check if in linear tolerance distance + geometry_msgs::Pose2D cell_world_pose; + costmap2d->mapToWorld(neighbor.x, neighbor.y, cell_world_pose.x, cell_world_pose.y); + if (std::hypot(cell_world_pose.x - config_.goal.x, cell_world_pose.y - config_.goal.y) > config_.linear_tolerance) + { + ROS_DEBUG_STREAM("Cell " << neighbor.x << ", " << neighbor.y + << " is not within linear tolerance of goal; skipping"); + continue; + } + ROS_DEBUG_STREAM("Adding cell " << neighbor.x << ", " << neighbor.y << " to queue"); + in_queue_or_visited.insert(cell_index); + queue.push(neighbor); + } + } + + if (outside_or_unknown) + { + // the solution is a no information pose or outside + ROS_DEBUG_STREAM_COND_NAMED(sol.search_state.state == SearchState::UNKNOWN, LOGNAME.data(), + "The best solution found has NO_INFORMATION cost"); + ROS_DEBUG_STREAM_COND_NAMED(sol.search_state.state == SearchState::OUTSIDE, LOGNAME.data(), + "The best solution found is OUTSIDE the map"); + if (viz_) + { + viz_->addSolution(sol.pose, footprint); + viz_->publish(); + } + return sol; + } + + ROS_DEBUG_STREAM("No solution found within tolerance of goal; ending search"); + if (viz_) + { + viz_->publish(); + } + sol.search_state.state = SearchState::LETHAL; + sol.search_state.cost = costmap_2d::LETHAL_OBSTACLE; + sol.pose = config_.goal; + return sol; +} +} /* namespace mbf_costmap_nav */ diff --git a/mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search_viz.cpp b/mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search_viz.cpp new file mode 100644 index 00000000..a6effa8c --- /dev/null +++ b/mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search_viz.cpp @@ -0,0 +1,127 @@ +/* + * Copyright 2023, Rapyuta Robotics Co., Ltd., Renan Salles + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * free_pose_search_viz.cpp + * + * authors: + * Renan Salles + * + */ + +// mbf +#include "mbf_costmap_nav/free_pose_search_viz.h" + +// ros +#include +#include + +namespace mbf_costmap_nav +{ +FreePoseSearchViz::FreePoseSearchViz(ros::NodeHandle& pnh, const std::string& frame_id) + : pnh_(pnh) + , frame_id_(frame_id) + , marker_pub_(pnh_.advertise("search_markers", 1, false)) +{ +} + +void FreePoseSearchViz::addMarker(const geometry_msgs::Pose2D& pose_2d, + const std::vector& footprint, const std::string& ns, + const std_msgs::ColorRGBA& color) +{ + tf2::Quaternion q; + q.setRPY(0, 0, pose_2d.theta); + + visualization_msgs::Marker marker; + marker.header.frame_id = frame_id_; + marker.header.stamp = ros::Time::now(); + marker.ns = ns; + marker.id = marker_id_++; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = pose_2d.x; + marker.pose.position.y = pose_2d.y; + marker.pose.position.z = 0; + marker.pose.orientation = tf2::toMsg(q); + marker.scale.x = 0.05; + marker.color = color; + marker.lifetime = ros::Duration(0); + + for (const auto& point : footprint) + { + geometry_msgs::Point p; + p.x = point.x; + p.y = point.y; + p.z = 0; + marker.points.push_back(p); + } + marker.points.push_back(marker.points.front()); + marker_array_.markers.push_back(marker); +} + +void FreePoseSearchViz::deleteMarkers() +{ + visualization_msgs::MarkerArray marker_array; + visualization_msgs::Marker marker; + marker.action = visualization_msgs::Marker::DELETEALL; + marker.ns = BLOCKED_NS; + marker_array.markers.push_back(marker); + marker.ns = SOLUTION_NS; + marker_array.markers.push_back(marker); + marker_pub_.publish(marker_array); +} + +void FreePoseSearchViz::addBlocked(const geometry_msgs::Pose2D& pose_2d, + const std::vector& footprint) +{ + std_msgs::ColorRGBA color; + color.r = 1; + color.a = 0.5; + addMarker(pose_2d, footprint, BLOCKED_NS, color); +} + +void FreePoseSearchViz::addSolution(const geometry_msgs::Pose2D& pose_2d, + const std::vector& footprint) +{ + std_msgs::ColorRGBA color; + color.g = 1; + color.a = 0.5; + addMarker(pose_2d, footprint, SOLUTION_NS, color); +} + +void FreePoseSearchViz::publish() +{ + marker_pub_.publish(marker_array_); + ROS_DEBUG("Published %zu markers", marker_array_.markers.size()); + marker_array_.markers.clear(); + marker_id_ = 0; +} +} // namespace mbf_costmap_nav diff --git a/mbf_costmap_nav/test/config.yaml b/mbf_costmap_nav/test/config.yaml new file mode 100644 index 00000000..27d9247e --- /dev/null +++ b/mbf_costmap_nav/test/config.yaml @@ -0,0 +1,7 @@ +search: + global: + track_unknown_space: false + footprint: [[-0.6, -0.6], [1.6, -0.6], [1.6, 0.6], [-0.6, 0.6]] # in Y it is a bit larger than one cell + plugins: + - {name: static, type: "costmap_2d::StaticLayer"} + - {name: obstacles, type: "costmap_2d::ObstacleLayer"} diff --git a/mbf_costmap_nav/test/free_pose_search.test b/mbf_costmap_nav/test/free_pose_search.test new file mode 100644 index 00000000..cc1e884d --- /dev/null +++ b/mbf_costmap_nav/test/free_pose_search.test @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/mbf_costmap_nav/test/free_pose_search_test.cpp b/mbf_costmap_nav/test/free_pose_search_test.cpp new file mode 100644 index 00000000..7d230e10 --- /dev/null +++ b/mbf_costmap_nav/test/free_pose_search_test.cpp @@ -0,0 +1,506 @@ +/* + * Copyright 2023, Rapyuta Robotics Co., Ltd., Renan Salles + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * free_pose_search_test.cpp + * + * authors: + * Renan Salles + * + */ + +// ros +#include +#include +#include +#include +#include +#include + +// mbf +#include "mbf_costmap_nav/free_pose_search.h" +#include "mbf_costmap_nav/costmap_navigation_server.h" + +namespace mbf_costmap_nav::test +{ +class SearchHelperTest : public ::testing::Test +{ +protected: + boost::shared_ptr tf_buffer_ptr; + tf2_ros::TransformListener tf_listener; + costmap_2d::Costmap2D costmap; + ros::Subscriber map_sub; + nav_msgs::OccupancyGrid map; + ros::Publisher map_pub; + + SearchHelperTest() + : tf_buffer_ptr(boost::make_shared()) + , tf_listener(*tf_buffer_ptr) + , costmap(10, 10, 1.0, 0.0, 0.0, 0) + { + tf_buffer_ptr->setUsingDedicatedThread(true); + } + + void SetUp() override + { + if (!tf_buffer_ptr->canTransform("base_link", "map", ros::Time::now(), ros::Duration(5.0))) + { + FAIL() << "Cannot transform from base_link to map"; + } + map_sub = ros::NodeHandle().subscribe("map", 1, &SearchHelperTest::mapCb, this); + map_pub = ros::NodeHandle().advertise("map", 1, true); + } + + void mapCb(const nav_msgs::OccupancyGrid& m) + { + map = m; + } + + geometry_msgs::Point toPoint(double x, double y) + { + geometry_msgs::Point point; + point.x = x; + point.y = y; + return point; + } + + geometry_msgs::Pose2D toPose2D(double x, double y, double theta) + { + geometry_msgs::Pose2D pose; + pose.x = x; + pose.y = y; + pose.theta = theta; + return pose; + } + + void addObstacle(costmap_2d::Costmap2DROS& cm, double x, double y) + { + boost::shared_ptr olayer; + std::vector >* plugins = cm.getLayeredCostmap()->getPlugins(); + for (std::vector >::iterator pluginp = plugins->begin(); + pluginp != plugins->end(); ++pluginp) + { + boost::shared_ptr plugin = *pluginp; + if (plugin->getName().find("obstacles") != std::string::npos) + { + olayer = boost::static_pointer_cast(plugin); + addObservation(&(*olayer), x, y, MAX_Z / 2, 0, 0, MAX_Z / 2); + } + } + + cm.updateMap(); + printMap(*(cm.getCostmap())); + } +}; + +TEST_F(SearchHelperTest, getNeighbors) +{ + Cell cell{ 0, 0 }; + auto neighbors = FreePoseSearch::getNeighbors(costmap, cell); + ASSERT_EQ(neighbors.size(), 3); + EXPECT_EQ(neighbors[0].x, 0); + EXPECT_EQ(neighbors[0].y, 1); + EXPECT_EQ(neighbors[1].x, 1); + EXPECT_EQ(neighbors[1].y, 0); + EXPECT_EQ(neighbors[2].x, 1); + EXPECT_EQ(neighbors[2].y, 1); + + cell = { 9, 9 }; + neighbors = FreePoseSearch::getNeighbors(costmap, cell); + ASSERT_EQ(neighbors.size(), 3); + EXPECT_EQ(neighbors[0].x, 8); + EXPECT_EQ(neighbors[0].y, 8); + EXPECT_EQ(neighbors[1].x, 8); + EXPECT_EQ(neighbors[1].y, 9); + EXPECT_EQ(neighbors[2].x, 9); + EXPECT_EQ(neighbors[2].y, 8); + + cell = { 5, 5 }; + neighbors = FreePoseSearch::getNeighbors(costmap, cell); + ASSERT_EQ(neighbors.size(), 8); + EXPECT_EQ(neighbors[0].x, 4); + EXPECT_EQ(neighbors[0].y, 4); + EXPECT_EQ(neighbors[1].x, 4); + EXPECT_EQ(neighbors[1].y, 5); + EXPECT_EQ(neighbors[2].x, 4); + EXPECT_EQ(neighbors[2].y, 6); + EXPECT_EQ(neighbors[3].x, 5); + EXPECT_EQ(neighbors[3].y, 4); + EXPECT_EQ(neighbors[4].x, 5); + EXPECT_EQ(neighbors[4].y, 6); + EXPECT_EQ(neighbors[5].x, 6); + EXPECT_EQ(neighbors[5].y, 4); + EXPECT_EQ(neighbors[6].x, 6); + EXPECT_EQ(neighbors[6].y, 5); + EXPECT_EQ(neighbors[7].x, 6); + EXPECT_EQ(neighbors[7].y, 6); + + cell = { 0, 5 }; + neighbors = FreePoseSearch::getNeighbors(costmap, cell); + ASSERT_EQ(neighbors.size(), 5); + EXPECT_EQ(neighbors[0].x, 0); + EXPECT_EQ(neighbors[0].y, 4); + EXPECT_EQ(neighbors[1].x, 0); + EXPECT_EQ(neighbors[1].y, 6); + EXPECT_EQ(neighbors[2].x, 1); + EXPECT_EQ(neighbors[2].y, 4); + EXPECT_EQ(neighbors[3].x, 1); + EXPECT_EQ(neighbors[3].y, 5); + EXPECT_EQ(neighbors[4].x, 1); + EXPECT_EQ(neighbors[4].y, 6); + + cell = { 5, 0 }; + neighbors = FreePoseSearch::getNeighbors(costmap, cell); + ASSERT_EQ(neighbors.size(), 5); + EXPECT_EQ(neighbors[0].x, 4); + EXPECT_EQ(neighbors[0].y, 0); + EXPECT_EQ(neighbors[1].x, 4); + EXPECT_EQ(neighbors[1].y, 1); + EXPECT_EQ(neighbors[2].x, 5); + EXPECT_EQ(neighbors[2].y, 1); + EXPECT_EQ(neighbors[3].x, 6); + EXPECT_EQ(neighbors[3].y, 0); + EXPECT_EQ(neighbors[4].x, 6); + EXPECT_EQ(neighbors[4].y, 1); +} + +TEST_F(SearchHelperTest, safetyPadding) +{ + costmap_2d::Costmap2DROS cm("unpadded", *tf_buffer_ptr); + + auto footprint = FreePoseSearch::safetyPadding(cm, true, 0.1); + EXPECT_EQ(3, footprint.size()); + EXPECT_NEAR(1.1f, footprint[0].x, 1e-6); + EXPECT_NEAR(1.1f, footprint[0].y, 1e-6); + EXPECT_NEAR(-1.1f, footprint[1].x, 1e-6); + EXPECT_NEAR(1.1f, footprint[1].y, 1e-6); + EXPECT_NEAR(-1.1f, footprint[2].x, 1e-6); + EXPECT_NEAR(-1.1f, footprint[2].y, 1e-6); + + footprint = FreePoseSearch::safetyPadding(cm, false, 0.1); + EXPECT_EQ(3, footprint.size()); + EXPECT_NEAR(1.1f, footprint[0].x, 1e-6); + EXPECT_NEAR(1.1f, footprint[0].y, 1e-6); + EXPECT_NEAR(-1.1f, footprint[1].x, 1e-6); + EXPECT_NEAR(1.1f, footprint[1].y, 1e-6); + EXPECT_NEAR(-1.1f, footprint[2].x, 1e-6); + EXPECT_NEAR(-1.1f, footprint[2].y, 1e-6); + + costmap_2d::Costmap2DROS cm2("padded", *tf_buffer_ptr); + footprint = FreePoseSearch::safetyPadding(cm2, true, 0.1); + EXPECT_EQ(3, footprint.size()); + EXPECT_NEAR(1.6f, footprint[0].x, 1e-6); + EXPECT_NEAR(1.6f, footprint[0].y, 1e-6); + EXPECT_NEAR(-1.6f, footprint[1].x, 1e-6); + EXPECT_NEAR(1.6f, footprint[1].y, 1e-6); + EXPECT_NEAR(-1.6f, footprint[2].x, 1e-6); + EXPECT_NEAR(-1.6f, footprint[2].y, 1e-6); + + footprint = FreePoseSearch::safetyPadding(cm2, false, 0.1); + EXPECT_EQ(3, footprint.size()); + EXPECT_NEAR(1.1f, footprint[0].x, 1e-6); + EXPECT_NEAR(1.1f, footprint[0].y, 1e-6); + EXPECT_NEAR(-1.1f, footprint[1].x, 1e-6); + EXPECT_NEAR(1.1f, footprint[1].y, 1e-6); + EXPECT_NEAR(-1.1f, footprint[2].x, 1e-6); + EXPECT_NEAR(-1.1f, footprint[2].y, 1e-6); +} + +TEST_F(SearchHelperTest, getFootprintState) +{ + std::vector footprint = { toPoint(-0.5, -0.5), toPoint(0.5, -0.5), toPoint(0.5, 0.5), + toPoint(-0.5, 0.5) }; + + auto test = [&](std::uint8_t state) { + EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(5, 5, 0)).state, state); + EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.6, 5, 0)).state, state); + EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.6, 4.6, 0)).state, state); + EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.6, 4.6, M_PI)).state, state); + EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.5, 4.5, 0)).state, state); + EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.4, 4.4, 0)).state, SearchState::FREE); + EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.5, 4.5, M_PI_4)).state, + SearchState::FREE); + EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(3, 5, 0)).state, SearchState::FREE); + }; + + // Test LETHAL + costmap.setCost(5, 5, costmap_2d::LETHAL_OBSTACLE); + test(SearchState::LETHAL); + + // Test NO_INFORMATION + costmap.setCost(5, 5, costmap_2d::NO_INFORMATION); + test(SearchState::UNKNOWN); + + // Test INSCRIBED + costmap.setCost(5, 5, costmap_2d::FREE_SPACE); + costmap.setCost(5, 6, costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + costmap.setCost(6, 5, costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + costmap.setCost(5, 4, costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + costmap.setCost(4, 5, costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(5, 5, 0)).state, SearchState::INSCRIBED); + + // Test OUTSIDE + EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(0, 0, 0)).state, SearchState::OUTSIDE); +} + +TEST_F(SearchHelperTest, findValidOrientation) +{ + costmap.setCost(5, 5, costmap_2d::LETHAL_OBSTACLE); + std::optional viz; + + // square + std::vector footprint = { toPoint(-0.5, -0.5), toPoint(0.5, -0.5), toPoint(0.5, 0.5), + toPoint(-0.5, 0.5) }; + auto sol = FreePoseSearch::findValidOrientation(costmap, footprint, toPose2D(4.6, 4.6, 0), { M_PI_4 / 2, M_PI }, viz); + EXPECT_EQ(sol.search_state.state, SearchState::FREE); + EXPECT_NEAR(sol.pose.theta, M_PI_4 / 2, 1e-6); + + sol = FreePoseSearch::findValidOrientation(costmap, footprint, toPose2D(4.6, 4.6, 0), { M_PI, 2 * M_PI }, viz); + EXPECT_EQ(sol.search_state.state, SearchState::LETHAL); + + // rectangle + footprint = { toPoint(-0.5, -0.4), toPoint(1.0, -0.4), toPoint(1.0, 0.4), toPoint(-0.5, 0.4) }; + + sol = FreePoseSearch::findValidOrientation(costmap, footprint, toPose2D(4.5, 5.5, 0), { M_PI_4, M_PI }, viz); + EXPECT_EQ(sol.search_state.state, SearchState::FREE); + EXPECT_NEAR(sol.pose.theta, M_PI_2, 1e-6); + + sol = FreePoseSearch::findValidOrientation(costmap, footprint, toPose2D(4.5, 5.5, -M_PI_2), { M_PI_4, M_PI }, viz); + EXPECT_EQ(sol.search_state.state, SearchState::FREE); + EXPECT_NEAR(sol.pose.theta, -M_PI_2, 1e-6); + + // inscribed + costmap.setCost(4, 4, costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + costmap.setCost(4, 6, costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + sol = FreePoseSearch::findValidOrientation(costmap, footprint, toPose2D(4.5, 5.5, 0), { M_PI_4, M_PI_2 }, viz); + EXPECT_EQ(sol.search_state.state, SearchState::INSCRIBED); + EXPECT_NEAR(sol.pose.theta, M_PI_2, 1e-6); +} + +TEST_F(SearchHelperTest, search) +{ + ros::NodeHandle nh; + costmap_2d::Costmap2DROS cm("search/global", *tf_buffer_ptr); + FreePoseSearchViz viz(nh, cm.getGlobalFrameID()); + + printMap(*(cm.getCostmap())); + addObstacle(cm, 5.5, 5.5); + map.header.stamp = ros::Time::now(); + map.data[cm.getCostmap()->getIndex(5, 5)] = 100; + map_pub.publish(map); + + /* + y/x 0.5 1.5 2.5 3.5 4.5 5.5 6.5 7.5 8.5 9.5 + ------------------------------------------------------------ + 0.5 | 0 0 0 0 0 0 0 254 254 254 + 1.5 | 0 0 0 0 0 0 0 254 254 254 + 2.5 | 0 0 0 254 254 254 0 0 0 0 + 3.5 | 0 0 0 0 0 0 0 0 0 0 + 4.5 | 0 0 0 0 0 0 x 0 0 0 + 5.5 | 0 0 0 0 254 G 0 254 254 254 + 6.5 | 0 0 0 0 254 0 0 254 254 254 + 7.5 | 0 0 0 0 0 0 0 254 254 254 + 8.5 | 0 0 0 0 0 0 0 0 0 0 + 9.5 | 0 0 0 0 0 0 0 0 0 0 + */ + + SearchConfig config{ M_PI_4, M_PI, 5.0, false, 0.0, toPose2D(5.5, 5.5, 0) }; + FreePoseSearch sh(cm, config, std::nullopt, viz); + + auto sol = sh.search(); + EXPECT_EQ(sol.search_state.state, SearchState::FREE); + EXPECT_NEAR(sol.pose.x, 6.5, 1e-6); + EXPECT_NEAR(sol.pose.y, 4.5, 1e-6); + EXPECT_NEAR(sol.pose.theta, -M_PI_4, 1e-6); + + addObstacle(cm, 6.5, 4.5); + map.header.stamp = ros::Time::now(); + map.data[cm.getCostmap()->getIndex(6, 4)] = 100; + map_pub.publish(map); + + /* + y/x 0.5 1.5 2.5 3.5 4.5 5.5 6.5 7.5 8.5 9.5 + ------------------------------------------------------------ + 0.5 | 0 0 0 0 0 0 0 254 254 254 + 1.5 | 0 0 0 0 0 0 0 254 254 254 + 2.5 | 0 0 0 254 254 254 0 0 0 0 + 3.5 | 0 0 0 0 254 0 0 0 0 0 + 4.5 | 0 0 0 0 0 0 254 0 0 0 + 5.5 | 0 0 0 0 254 G 0 254 254 254 + 6.5 | 0 0 0 0 254 0 0 254 254 254 + 7.5 | 0 0 0 0 0 x 0 254 254 254 + 8.5 | 0 0 0 0 0 0 0 0 0 0 + 9.5 | 0 0 0 0 0 0 0 0 0 0 + */ + + sol = sh.search(); + EXPECT_EQ(sol.search_state.state, SearchState::FREE); + EXPECT_NEAR(sol.pose.x, 5.5, 1e-6); + EXPECT_NEAR(sol.pose.y, 7.5, 1e-6); + EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6); + + addObstacle(cm, 5.5, 7.5); + map.header.stamp = ros::Time::now(); + map.data[cm.getCostmap()->getIndex(5, 7)] = 100; + map_pub.publish(map); + + /* + y/x 0.5 1.5 2.5 3.5 4.5 5.5 6.5 7.5 8.5 9.5 + ------------------------------------------------------------ + 0.5 | 0 0 0 0 0 0 0 254 254 254 + 1.5 | 0 0 0 0 0 0 0 254 254 254 + 2.5 | 0 0 0 254 254 254 0 0 0 0 + 3.5 | 0 0 0 0 254 0 0 0 0 0 + 4.5 | 0 0 0 x 0 0 254 0 0 0 + 5.5 | 0 0 0 0 254 G 0 254 254 254 + 6.5 | 0 0 0 0 254 0 0 254 254 254 + 7.5 | 0 0 0 0 0 254 0 254 254 254 + 8.5 | 0 0 0 0 0 0 0 0 0 0 + 9.5 | 0 0 0 0 0 0 0 0 0 0 + */ + + sol = sh.search(); + EXPECT_EQ(sol.search_state.state, SearchState::FREE); + EXPECT_NEAR(sol.pose.x, 3.5, 1e-6); + EXPECT_NEAR(sol.pose.y, 4.5, 1e-6); + EXPECT_NEAR(sol.pose.theta, 3 * M_PI_4, 1e-6); + + addObstacle(cm, 3.5, 4.5); + map.header.stamp = ros::Time::now(); + map.data[cm.getCostmap()->getIndex(3, 4)] = 100; + map_pub.publish(map); + + /* + y/x 0.5 1.5 2.5 3.5 4.5 5.5 6.5 7.5 8.5 9.5 + ------------------------------------------------------------ + 0.5 | 0 0 0 0 0 0 0 254 254 254 + 1.5 | 0 0 0 0 0 0 0 254 254 254 + 2.5 | 0 0 0 254 254 254 0 0 0 0 + 3.5 | 0 0 0 0 254 0 0 0 0 0 + 4.5 | 0 0 0 254 0 0 254 0 0 0 + 5.5 | 0 0 0 0 254 G 0 254 254 254 + 6.5 | 0 0 0 0 254 0 0 254 254 254 + 7.5 | 0 0 0 x 0 254 0 254 254 254 + 8.5 | 0 0 0 0 0 0 0 0 0 0 + 9.5 | 0 0 0 0 0 0 0 0 0 0 + */ + + sol = sh.search(); + EXPECT_EQ(sol.search_state.state, SearchState::FREE); + EXPECT_NEAR(sol.pose.x, 3.5, 1e-6); + EXPECT_NEAR(sol.pose.y, 7.5, 1e-6); + EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6); + + // Non-zero orientation + SearchConfig config2{ M_PI_4, M_PI, 5.0, false, 0.0, toPose2D(1.5, 4.5, M_PI_4) }; + FreePoseSearch sh2(cm, config2, std::nullopt, viz); + sol = sh2.search(); + EXPECT_EQ(sol.search_state.state, SearchState::FREE); + EXPECT_NEAR(sol.pose.x, 1.5, 1e-6); + EXPECT_NEAR(sol.pose.y, 4.5, 1e-6); + EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6); +} + +TEST_F(SearchHelperTest, service_test) +{ + CostmapNavigationServer server(tf_buffer_ptr); + + ros::ServiceClient client = ros::NodeHandle("~").serviceClient("find_valid_pose"); + mbf_msgs::FindValidPose::Request req; + mbf_msgs::FindValidPose::Response res; + + req.angle_tolerance = M_PI_4; + req.dist_tolerance = 1.0; + req.use_padded_fp = false; + req.costmap = mbf_msgs::FindValidPose::Request::GLOBAL_COSTMAP; + req.pose.header.frame_id = "map"; + req.pose.header.stamp = ros::Time::now(); + req.pose.pose.position.x = 1.525; + req.pose.pose.position.y = 4.525; + req.pose.pose.position.z = 0.0; + req.pose.pose.orientation = tf::createQuaternionMsgFromYaw(M_PI_4); + + ASSERT_TRUE(client.call(req, res)); + EXPECT_EQ(res.state, mbf_msgs::FindValidPose::Response::FREE); + EXPECT_NEAR(res.pose.pose.position.x, 1.525, 1e-6); + EXPECT_NEAR(res.pose.pose.position.y, 4.525, 1e-6); + EXPECT_NEAR(tf::getYaw(res.pose.pose.orientation), M_PI_4, 1e-6); + server.stop(); +} + +TEST_F(SearchHelperTest, enforce_bounds) +{ + ros::NodeHandle nh; + costmap_2d::Costmap2DROS cm("search/global", *tf_buffer_ptr); + FreePoseSearchViz viz(nh, cm.getGlobalFrameID()); + + printMap(*(cm.getCostmap())); + + /* + y/x 0.5 1.5 2.5 3.5 4.5 5.5 6.5 7.5 8.5 9.5 + ------------------------------------------------------------ + 0.5 | G 0 0 0 0 0 0 254 254 254 + 1.5 | 0 0 0 0 0 0 0 254 254 254 + 2.5 | 0 0 0 254 254 254 0 0 0 0 + 3.5 | 0 0 0 0 0 0 0 0 0 0 + 4.5 | 0 0 0 0 0 0 0 0 0 0 + 5.5 | 0 0 0 0 254 0 0 254 254 254 + 6.5 | 0 0 0 0 254 0 0 254 254 254 + 7.5 | 0 0 0 0 0 0 0 254 254 254 + 8.5 | 0 0 0 0 0 0 0 0 0 0 + 9.5 | 0 0 0 0 0 0 0 0 0 0 + */ + + SearchConfig config{ M_PI_4, M_PI, 0.5, false, 0.0, toPose2D(-1, -1, 0) }; + FreePoseSearch sh(cm, config, std::nullopt, viz); + + auto sol = sh.search(); + EXPECT_EQ(sol.search_state.state, SearchState::OUTSIDE); + EXPECT_NEAR(sol.pose.x, 0.5, 1e-6); + EXPECT_NEAR(sol.pose.y, 0.5, 1e-6); + EXPECT_NEAR(sol.pose.theta, 0, 1e-6); +} +} // namespace mbf_costmap_nav::test + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "search_helper_test"); + // enable debug logging for tests + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) + ros::console::notifyLoggerLevelsChanged(); + + ros::AsyncSpinner spinner(0); + spinner.start(); + testing::InitGoogleTest(&argc, argv); + auto result = RUN_ALL_TESTS(); + spinner.stop(); + return result; +} diff --git a/mbf_costmap_nav/test/record_bag_file.sh b/mbf_costmap_nav/test/record_bag_file.sh new file mode 100755 index 00000000..c43dd66e --- /dev/null +++ b/mbf_costmap_nav/test/record_bag_file.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash + +# Record topics required to debug tests + +OUTPUT_PATH="$(echo $1 | sed 's![^/]$!&/!')" +echo "Bag file will be saved in "$OUTPUT_PATH + +rosbag record -o "$OUTPUT_PATH"mbf_test \ + -e "(.*)/search_markers(.*)" \ + -e "(.*)/map(.*)" \ diff --git a/mbf_msgs/CMakeLists.txt b/mbf_msgs/CMakeLists.txt index 6f61f920..7320bf8f 100644 --- a/mbf_msgs/CMakeLists.txt +++ b/mbf_msgs/CMakeLists.txt @@ -19,6 +19,7 @@ add_service_files( CheckPoint.srv CheckPose.srv CheckPath.srv + FindValidPose.srv ) add_action_files(