Skip to content

Commit cf2ab03

Browse files
SteveMacenskiSteveMacenski
authored
Fix zig-zaggin in smac planner due to discrete angular bin quantizations (ros-navigation#3752)
* adding intial prototype for smac hybrid-A* multiple primitives quantization interpolation * fixing unsigned int issue * cleaning up expansions log * fix primitives to be of equal dist * adding test for expansions getting * fixing tests from changes * adding a theoretical guardrail * updating Hybrid-A* to include directional information in the primitives for enabling multi-primitive planning & accurate traversal cost functions * moving Hybrid-A* specifics to template specialization * expand support for debug visualizations to State Lattice and 2D A*'s * fix some test oddities * dynamic parameters * remove unused variable * minor changes * adding options for downsampling obstacle heurisic and for using quadratic cost penalty functions * fixing unit tests
1 parent 910a9f2 commit cf2ab03

12 files changed

Lines changed: 370 additions & 81 deletions

File tree

‎nav2_smac_planner/include/nav2_smac_planner/a_star.hpp‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -110,7 +110,7 @@ class AStarAlgorithm
110110
*/
111111
bool createPath(
112112
CoordinateVector & path, int & num_iterations, const float & tolerance,
113-
std::vector<std::tuple<float, float>> * expansions_log = nullptr);
113+
std::vector<std::tuple<float, float, float>> * expansions_log = nullptr);
114114

115115
/**
116116
* @brief Sets the collision checker to use
@@ -240,6 +240,14 @@ class AStarAlgorithm
240240
*/
241241
inline void clearGraph();
242242

243+
/**
244+
* @brief Populate a debug log of expansions for Hybrid-A* for visualization
245+
* @param node Node expanded
246+
* @param expansions_log Log to add not expanded to
247+
*/
248+
inline void populateExpansionsLog(
249+
const NodePtr & node, std::vector<std::tuple<float, float, float>> * expansions_log);
250+
243251
int _timing_interval = 5000;
244252

245253
bool _traverse_unknown;

‎nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,7 @@ class NodeBasic
7676
MotionPrimitive * prim_ptr; // Used by NodeLattice
7777
unsigned int index, motion_index;
7878
bool backward;
79+
TurnDirection turn_dir;
7980
};
8081

8182
} // namespace nav2_smac_planner

‎nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp‎

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -122,10 +122,13 @@ struct HybridMotionTable
122122
float cost_penalty;
123123
float reverse_penalty;
124124
float travel_distance_reward;
125+
bool downsample_obstacle_heuristic;
126+
bool use_quadratic_cost_penalty;
125127
ompl::base::StateSpacePtr state_space;
126128
std::vector<std::vector<double>> delta_xs;
127129
std::vector<std::vector<double>> delta_ys;
128130
std::vector<TrigValues> trig_values;
131+
std::vector<float> travel_costs;
129132
};
130133

131134
/**
@@ -232,9 +235,10 @@ class NodeHybrid
232235
* @brief Sets the motion primitive index used to achieve node in search
233236
* @param reference to motion primitive idx
234237
*/
235-
inline void setMotionPrimitiveIndex(const unsigned int & idx)
238+
inline void setMotionPrimitiveIndex(const unsigned int & idx, const TurnDirection & turn_dir)
236239
{
237240
_motion_primitive_index = idx;
241+
_turn_dir = turn_dir;
238242
}
239243

240244
/**
@@ -246,6 +250,15 @@ class NodeHybrid
246250
return _motion_primitive_index;
247251
}
248252

253+
/**
254+
* @brief Gets the motion primitive turning direction used to achieve node in search
255+
* @return reference to motion primitive turning direction
256+
*/
257+
inline TurnDirection & getTurnDirection()
258+
{
259+
return _turn_dir;
260+
}
261+
249262
/**
250263
* @brief Gets the costmap cost at this node
251264
* @return costmap cost
@@ -460,6 +473,7 @@ class NodeHybrid
460473
unsigned int _index;
461474
bool _was_visited;
462475
unsigned int _motion_primitive_index;
476+
TurnDirection _turn_dir;
463477
};
464478

465479
} // namespace nav2_smac_planner

‎nav2_smac_planner/include/nav2_smac_planner/types.hpp‎

Lines changed: 35 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -34,18 +34,21 @@ typedef std::pair<float, unsigned int> NodeHeuristicPair;
3434
*/
3535
struct SearchInfo
3636
{
37-
float minimum_turning_radius;
38-
float non_straight_penalty;
39-
float change_penalty;
40-
float reverse_penalty;
41-
float cost_penalty;
42-
float retrospective_penalty;
43-
float rotation_penalty;
44-
float analytic_expansion_ratio;
45-
float analytic_expansion_max_length;
37+
float minimum_turning_radius{8.0};
38+
float non_straight_penalty{1.05};
39+
float change_penalty{0.0};
40+
float reverse_penalty{2.0};
41+
float cost_penalty{2.0};
42+
float retrospective_penalty{0.015};
43+
float rotation_penalty{5.0};
44+
float analytic_expansion_ratio{3.5};
45+
float analytic_expansion_max_length{60.0};
4646
std::string lattice_filepath;
47-
bool cache_obstacle_heuristic;
48-
bool allow_reverse_expansion;
47+
bool cache_obstacle_heuristic{false};
48+
bool allow_reverse_expansion{false};
49+
bool allow_primitive_interpolation{false};
50+
bool downsample_obstacle_heuristic{true};
51+
bool use_quadratic_cost_penalty{false};
4952
};
5053

5154
/**
@@ -101,6 +104,21 @@ struct SmootherParams
101104
int refinement_num_;
102105
};
103106

107+
/**
108+
* @struct nav2_smac_planner::TurnDirection
109+
* @brief A struct with the motion primitive's direction embedded
110+
*/
111+
enum struct TurnDirection
112+
{
113+
UNKNOWN = 0,
114+
FORWARD = 1,
115+
LEFT = 2,
116+
RIGHT = 3,
117+
REVERSE = 4,
118+
REV_LEFT = 5,
119+
REV_RIGHT = 6
120+
};
121+
104122
/**
105123
* @struct nav2_smac_planner::MotionPose
106124
* @brief A struct for poses in motion primitives
@@ -117,19 +135,22 @@ struct MotionPose
117135
* @param x X pose
118136
* @param y Y pose
119137
* @param theta Angle of pose
138+
* @param TurnDirection Direction of the primitive's turn
120139
*/
121-
MotionPose(const float & x, const float & y, const float & theta)
122-
: _x(x), _y(y), _theta(theta)
140+
MotionPose(const float & x, const float & y, const float & theta, const TurnDirection & turn_dir)
141+
: _x(x), _y(y), _theta(theta), _turn_dir(turn_dir)
123142
{}
124143

125144
MotionPose operator-(const MotionPose & p2)
126145
{
127-
return MotionPose(this->_x - p2._x, this->_y - p2._y, this->_theta - p2._theta);
146+
return MotionPose(
147+
this->_x - p2._x, this->_y - p2._y, this->_theta - p2._theta, TurnDirection::UNKNOWN);
128148
}
129149

130150
float _x;
131151
float _y;
132152
float _theta;
153+
TurnDirection _turn_dir;
133154
};
134155

135156
typedef std::vector<MotionPose> MotionPoses;

‎nav2_smac_planner/src/a_star.cpp‎

Lines changed: 26 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -151,6 +151,30 @@ void AStarAlgorithm<NodeT>::setStart(
151151
static_cast<float>(dim_3)));
152152
}
153153

154+
template<>
155+
void AStarAlgorithm<Node2D>::populateExpansionsLog(
156+
const NodePtr & node,
157+
std::vector<std::tuple<float, float, float>> * expansions_log)
158+
{
159+
Node2D::Coordinates coords = node->getCoords(node->getIndex());
160+
expansions_log->emplace_back(
161+
_costmap->getOriginX() + ((coords.x + 0.5) * _costmap->getResolution()),
162+
_costmap->getOriginY() + ((coords.y + 0.5) * _costmap->getResolution()),
163+
0.0);
164+
}
165+
166+
template<typename NodeT>
167+
void AStarAlgorithm<NodeT>::populateExpansionsLog(
168+
const NodePtr & node,
169+
std::vector<std::tuple<float, float, float>> * expansions_log)
170+
{
171+
typename NodeT::Coordinates coords = node->pose;
172+
expansions_log->emplace_back(
173+
_costmap->getOriginX() + ((coords.x + 0.5) * _costmap->getResolution()),
174+
_costmap->getOriginY() + ((coords.y + 0.5) * _costmap->getResolution()),
175+
NodeT::motion_table.getAngleFromBin(coords.theta));
176+
}
177+
154178
template<>
155179
void AStarAlgorithm<Node2D>::setGoal(
156180
const unsigned int & mx,
@@ -222,7 +246,7 @@ template<typename NodeT>
222246
bool AStarAlgorithm<NodeT>::createPath(
223247
CoordinateVector & path, int & iterations,
224248
const float & tolerance,
225-
std::vector<std::tuple<float, float>> * expansions_log)
249+
std::vector<std::tuple<float, float, float>> * expansions_log)
226250
{
227251
steady_clock::time_point start_time = steady_clock::now();
228252
_tolerance = tolerance;
@@ -276,13 +300,7 @@ bool AStarAlgorithm<NodeT>::createPath(
276300

277301
// Save current node coordinates for debug
278302
if (expansions_log) {
279-
Coordinates coords = current_node->getCoords(
280-
current_node->getIndex(), getSizeX(), getSizeDim3());
281-
expansions_log->push_back(
282-
std::make_tuple<float, float>(
283-
_costmap->getOriginX() + (coords.x * _costmap->getResolution()),
284-
_costmap->getOriginY() + (coords.y * _costmap->getResolution()))
285-
);
303+
populateExpansionsLog(current_node, expansions_log);
286304
}
287305

288306
// We allow for nodes to be queued multiple times in case

‎nav2_smac_planner/src/node_basic.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ void NodeBasic<NodeHybrid>::processSearchNode()
3030
// a new branch is overriding one of lower cost already visited.
3131
if (!this->graph_node_ptr->wasVisited()) {
3232
this->graph_node_ptr->pose = this->pose;
33-
this->graph_node_ptr->setMotionPrimitiveIndex(this->motion_index);
33+
this->graph_node_ptr->setMotionPrimitiveIndex(this->motion_index, this->turn_dir);
3434
}
3535
}
3636

@@ -59,6 +59,7 @@ void NodeBasic<NodeHybrid>::populateSearchNode(NodeHybrid * & node)
5959
this->pose = node->pose;
6060
this->graph_node_ptr = node;
6161
this->motion_index = node->getMotionPrimitiveIndex();
62+
this->turn_dir = node->getTurnDirection();
6263
}
6364

6465
template<>

0 commit comments

Comments
 (0)