Skip to content

Commit

Permalink
Add search helper for goal angle tolerance (#317)
Browse files Browse the repository at this point in the history
  • Loading branch information
@renan028
renan028 authored Jul 5, 2023
1 parent e9519ed commit cc5351a
Show file tree
Hide file tree
Showing 15 changed files with 1,684 additions and 138 deletions.
21 changes: 21 additions & 0 deletions mbf_costmap_nav/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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})
Expand Down Expand Up @@ -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()
84 changes: 48 additions & 36 deletions mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@
#include <mbf_msgs/CheckPath.h>
#include <mbf_msgs/CheckPose.h>
#include <mbf_msgs/CheckPoint.h>
#include <mbf_msgs/FindValidPose.h>

#include <nav_core/base_global_planner.h>
#include <nav_core/base_local_planner.h>
Expand All @@ -71,8 +72,8 @@ namespace mbf_costmap_nav
* @brief Classes belonging to the Move Base Server level.
*/


typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_costmap_nav::MoveBaseFlexConfig> > DynamicReconfigureServerCostmapNav;
typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_costmap_nav::MoveBaseFlexConfig> >
DynamicReconfigureServerCostmapNav;

/// @brief A mapping from a string to a map-ptr.
typedef boost::unordered_map<std::string, CostmapWrapper::Ptr> StringToMap;
Expand All @@ -88,14 +89,13 @@ typedef boost::unordered_map<std::string, CostmapWrapper::Ptr> StringToMap;
class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServer
{
public:

typedef boost::shared_ptr<CostmapNavigationServer> 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
Expand All @@ -105,62 +105,56 @@ 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.
* @param plugin_name Name of the controller to use.
* @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.
* @param plugin_name Name of the recovery behavior to run.
* @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
* @param name The name of the planner
* @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
* @param controller_type The type of the controller plugin
* @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
Expand All @@ -169,69 +163,82 @@ 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
* @param name The name of the recovery behavior
* @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<std::string, CostmapWrapper::Ptr>
requestedCostmap(mbf_msgs::CheckPose::Request::_costmap_type costmap_type) const;

/**
* @brief Callback method for the check_point_cost service
* @param request Request object, see the mbf_msgs/CheckPoint service definition file.
* @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
* @param request Request object, see the mbf_msgs/CheckPose service definition file.
* @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
* @param request Request object, see the mbf_msgs/CheckPath service definition file.
* @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
* @param request Empty request object.
* @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<mbf_costmap_core::CostmapRecovery> recovery_plugin_loader_;
pluginlib::ClassLoader<nav_core::RecoveryBehavior> nav_core_recovery_plugin_loader_;
Expand Down Expand Up @@ -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 */
Expand Down
13 changes: 12 additions & 1 deletion mbf_costmap_nav/include/mbf_costmap_nav/footprint_helper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ namespace mbf_costmap_nav

struct Cell
{
unsigned int x, y;
unsigned int x, y, cost;
};

class FootprintHelper
Expand All @@ -66,6 +66,17 @@ class FootprintHelper
const std::vector<geometry_msgs::Point>& 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<Cell>& 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
Expand Down
Loading

0 comments on commit cc5351a

Please sign in to comment.