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..005dd205d80 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, float & mx, float & 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..9cb7ce19816 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, float & mx, float & my) const
+{
+ if (wx < origin_x_ || wy < origin_y_) {
+ return false;
+ }
+
+ mx = static_cast<float>((wx - origin_x_) / resolution_) + 0.5f;
+ my = static_cast<float>((wy - origin_y_) / resolution_) + 0.5f;
+
+ 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<int>((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 0470bfd3029..92a89230e92 100644
--- a/nav2_smac_planner/src/a_star.cpp
+++ b/nav2_smac_planner/src/a_star.cpp
@@ -132,28 +132,32 @@ typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::addToGraph(
template<>
void AStarAlgorithm<Node2D>::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<unsigned int>(mx),
+ static_cast<unsigned int>(my),
+ getSizeX()));
}
template<typename NodeT>
void AStarAlgorithm<NodeT>::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<float>(mx),
- static_cast<float>(my),
- static_cast<float>(dim_3)));
+ _start = addToGraph(
+ NodeT::getIndex(
+ static_cast<unsigned int>(mx),
+ static_cast<unsigned int>(my),
+ dim_3));
+ _start->setPose(Coordinates(mx, my, dim_3));
}
template<>
@@ -182,30 +186,35 @@ void AStarAlgorithm<NodeT>::populateExpansionsLog(
template<>
void AStarAlgorithm<Node2D>::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<unsigned int>(mx),
+ static_cast<unsigned int>(my),
+ getSizeX()));
_goal_coordinates = Node2D::Coordinates(mx, my);
}
template<typename NodeT>
void AStarAlgorithm<NodeT>::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<unsigned int>(mx),
+ static_cast<unsigned int>(my),
+ dim_3));
- typename NodeT::Coordinates goal_coords(
- static_cast<float>(mx),
- static_cast<float>(my),
- static_cast<float>(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..00cc07b464b 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");
@@ -244,7 +254,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
pose.pose.orientation.w = 1.0;
// Corner case of start and goal beeing on the same cell
- if (mx_start == mx_goal && my_start == my_goal) {
+ if (std::floor(mx_start) == std::floor(mx_goal) && std::floor(my_start) == std::floor(my_goal)) {
pose.pose = start.pose;
// if we have a different start and goal orientation, set the unique path pose to the goal
// orientation, unless use_final_approach_orientation=true where we need it to be the start
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");