From 6a233ed4a47feeb21f83e13e4ce5edf94ccc6541 Mon Sep 17 00:00:00 2001 From: Brice Date: Wed, 10 Apr 2024 13:27:24 +0000 Subject: [PATCH] Set start and goal as float Signed-off-by: Brice --- .../include/nav2_costmap_2d/costmap_2d.hpp | 10 ++++ nav2_costmap_2d/src/costmap_2d.cpp | 15 ++++++ .../include/nav2_smac_planner/a_star.hpp | 8 +-- nav2_smac_planner/src/a_star.cpp | 51 +++++++++++-------- nav2_smac_planner/src/smac_planner_2d.cpp | 16 ++++-- nav2_smac_planner/src/smac_planner_hybrid.cpp | 6 +-- .../src/smac_planner_lattice.cpp | 6 +-- 7 files changed, 78 insertions(+), 34 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 04b9f4daa43..15b6d65e38f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -183,6 +183,16 @@ class Costmap2D */ bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const; + /** + * @brief Convert from world coordinates to map coordinates + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @return True if the conversion was successful (legal bounds) false otherwise + */ + bool worldToMapContinuous(double wx, double wy, double & mx, double & my) const; + /** * @brief Convert from world coordinates to map coordinates without checking for legal bounds * @param wx The x world coordinate diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 8914166a5e1..de0bec1f6ad 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -296,6 +296,21 @@ bool Costmap2D::worldToMap(double wx, double wy, unsigned int & mx, unsigned int return false; } +bool Costmap2D::worldToMapContinuous(double wx, double wy, double & mx, double & my) const +{ + if (wx < origin_x_ || wy < origin_y_) { + return false; + } + + mx = (wx - origin_x_) / resolution_ + 0.5; + my = (wy - origin_y_) / resolution_ + 0.5; + + if (mx < size_x_ && my < size_y_) { + return true; + } + return false; +} + void Costmap2D::worldToMapNoBounds(double wx, double wy, int & mx, int & my) const { mx = static_cast((wx - origin_x_) / resolution_); diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 668aff5e4d8..b59fa9d66b6 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -129,8 +129,8 @@ class AStarAlgorithm * @param dim_3 The node dim_3 index of the goal */ void setGoal( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3); /** @@ -140,8 +140,8 @@ class AStarAlgorithm * @param dim_3 The node dim_3 index of the goal */ void setStart( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3); /** diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 1f0d7207631..86427a2428f 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -132,28 +132,32 @@ typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( template<> void AStarAlgorithm::setStart( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero starting dim 3."); } - _start = addToGraph(Node2D::getIndex(mx, my, getSizeX())); + _start = addToGraph( + Node2D::getIndex( + static_cast(mx), + static_cast(my), + getSizeX())); } template void AStarAlgorithm::setStart( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3) { - _start = addToGraph(NodeT::getIndex(mx, my, dim_3)); - _start->setPose( - Coordinates( - static_cast(mx), - static_cast(my), - static_cast(dim_3))); + _start = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + _start->setPose(Coordinates(mx, my, dim_3)); } template<> @@ -182,30 +186,35 @@ void AStarAlgorithm::populateExpansionsLog( template<> void AStarAlgorithm::setGoal( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } - _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX())); + _goal = addToGraph( + Node2D::getIndex( + static_cast(mx), + static_cast(my), + getSizeX())); _goal_coordinates = Node2D::Coordinates(mx, my); } template void AStarAlgorithm::setGoal( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3) { - _goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); + _goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + dim_3)); - typename NodeT::Coordinates goal_coords( - static_cast(mx), - static_cast(my), - static_cast(dim_3)); + typename NodeT::Coordinates goal_coords(mx, my, dim_3); if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { if (!_start) { diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 38fd12b9e17..27292837022 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -215,8 +215,13 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( _a_star->setCollisionChecker(&_collision_checker); // Set starting point - unsigned int mx_start, my_start, mx_goal, my_goal; - if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) { + float mx_start, my_start, mx_goal, my_goal; + if (!costmap->worldToMapContinuous( + start.pose.position.x, + start.pose.position.y, + mx_start, + my_start)) + { throw nav2_core::StartOutsideMapBounds( "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + std::to_string(start.pose.position.y) + ") was outside bounds"); @@ -224,7 +229,12 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( _a_star->setStart(mx_start, my_start, 0); // Set goal point - if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { + if (!costmap->worldToMapContinuous( + goal.pose.position.x, + goal.pose.position.y, + mx_goal, + my_goal)) + { throw nav2_core::GoalOutsideMapBounds( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index a41e794b597..0e0c8829222 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -355,8 +355,8 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates - unsigned int mx, my; - if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { + float mx, my; + if (!costmap->worldToMapContinuous(start.pose.position.x, start.pose.position.y, mx, my)) { throw nav2_core::StartOutsideMapBounds( "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + std::to_string(start.pose.position.y) + ") was outside bounds"); @@ -374,7 +374,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setStart(mx, my, orientation_bin_id); // Set goal point, in A* bin search coordinates - if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { + if (!costmap->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) { throw nav2_core::GoalOutsideMapBounds( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 87f2e365639..4f2dba45e43 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -281,8 +281,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates - unsigned int mx, my; - if (!_costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { + float mx, my; + if (!_costmap->worldToMapContinuous(start.pose.position.x, start.pose.position.y, mx, my)) { throw nav2_core::StartOutsideMapBounds( "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + std::to_string(start.pose.position.y) + ") was outside bounds"); @@ -292,7 +292,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation))); // Set goal point, in A* bin search coordinates - if (!_costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { + if (!_costmap->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) { throw nav2_core::GoalOutsideMapBounds( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds");