Skip to content

Commit 89d8744

Browse files
MichaelFYangMichaelFYang
committed
fix issues with dynamic environment
1 parent 4caaab8 commit 89d8744

15 files changed

+100
-103
lines changed

‎src/far_planner/config/default.yaml

+3-7
Original file line numberDiff line numberDiff line change
@@ -25,12 +25,10 @@ MapHandler/cell_length : 5.0 # Unit: meter
2525
MapHandler/map_grid_max_length : 1000.0 # Unit: meter
2626
MapHandler/map_grad_max_height : 100.0 # Unit: meter
2727

28-
# Scan Handler Params
29-
ScanHandler/inflate_scan_size : 2
30-
3128
# Dynamic Planner Utility Params
3229
Util/angle_noise : 15.0 # Unit: degree
3330
Util/accept_max_align_angle : 4.0 # Unit: degree
31+
Util/obs_inflate_size : 1
3432
Util/new_intensity_thred : 2.0
3533
Util/terrain_free_Z : 0.2
3634
Util/dyosb_update_thred : 4
@@ -40,14 +38,13 @@ Util/new_points_decay_time : 1.0 # Unit: second
4038

4139
# Dynamic Graph Params
4240
Graph/connect_votes_size : 10
43-
Graph/terrain_inflate_size : 1
4441
Graph/clear_dumper_thred : 4
4542
Graph/node_finalize_thred : 6
4643
Graph/filter_pool_size : 12
4744

4845
# Corner Detector Params
4946
CDetector/resize_ratio : 3.0
50-
CDetector/filter_count_value : 5
47+
CDetector/filter_count_value : 3
5148
CDetector/is_save_img : false
5249
CDetector/img_folder_path : /path
5350

@@ -56,5 +53,4 @@ GPlanner/converge_distance : 0.5 # Unit: meter
5653
GPlanner/goal_adjust_radius : 2.0 # Unit: meter
5754
GPlanner/free_counter_thred : 7
5855
GPlanner/reach_goal_vote_size : 3
59-
GPlanner/path_momentum_thred : 3
60-
GPlanner/clear_inflate_size : 3
56+
GPlanner/path_momentum_thred : 3

‎src/far_planner/config/matterport.yaml

+10-14
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,8 @@ main_run_freq : 2.5
33
voxel_dim : 0.1 # Unit: meter
44
robot_dim : 0.5 # Unit: meter
55
vehicle_height : 0.5 # Unit: meter
6-
sensor_range : 10.0 # Unit: meter
7-
terrain_range : 10.0 # Unit: meter
6+
sensor_range : 15.0 # Unit: meter
7+
terrain_range : 7.5 # Unit: meter
88
local_planner_range : 2.5 # Unit: meter
99
visualize_ratio : 0.4
1010
is_viewpoint_extend : true
@@ -21,40 +21,36 @@ GraphMsger/robot_id : 1 # graph from robot id "0" is extrac
2121

2222
# Map Handler Params
2323
MapHandler/floor_height : 2.0 # Unit: meter
24-
MapHandler/cell_length : 5.0 # Unit: meter
24+
MapHandler/cell_length : 2.5 # Unit: meter
2525
MapHandler/map_grid_max_length : 200.0 # Unit: meter
2626
MapHandler/map_grad_max_height : 10.0 # Unit: meter
2727

28-
# Scan Handler Params
29-
ScanHandler/inflate_scan_size : 2
30-
3128
# Dynamic Planner Utility Params
3229
Util/angle_noise : 15.0 # Unit: degree
3330
Util/accept_max_align_angle : 4.0 # Unit: degree
31+
Util/obs_inflate_size : 1
3432
Util/new_intensity_thred : 2.0
3533
Util/terrain_free_Z : 0.15
3634
Util/dyosb_update_thred : 4
3735
Util/new_point_counter : 5
3836
Util/dynamic_obs_dacay_time : 10.0 # Unit: second
39-
Util/new_points_decay_time : 2.0 # Unit: second
37+
Util/new_points_decay_time : 1.0 # Unit: second
4038

4139
# Dynamic Graph Params
42-
Graph/connect_votes_size : 10
43-
Graph/terrain_inflate_size : 1
40+
Graph/connect_votes_size : 10
4441
Graph/clear_dumper_thred : 4
4542
Graph/node_finalize_thred : 6
4643
Graph/filter_pool_size : 12
4744

4845
# Corner Detector Params
4946
CDetector/resize_ratio : 3.0
50-
CDetector/filter_count_value : 5
47+
CDetector/filter_count_value : 3
5148
CDetector/is_save_img : false
5249
CDetector/img_folder_path : /path
5350

5451
# Graph Planner Params
5552
GPlanner/converge_distance : 0.25 # Unit: meter
56-
GPlanner/goal_adjust_radius : 1.25 # Unit: meter
53+
GPlanner/goal_adjust_radius : 1.0 # Unit: meter
5754
GPlanner/free_counter_thred : 7
58-
GPlanner/reach_goal_vote_size : 5
59-
GPlanner/path_momentum_thred : 3
60-
GPlanner/clear_inflate_size : 3
55+
GPlanner/reach_goal_vote_size : 3
56+
GPlanner/path_momentum_thred : 3

‎src/far_planner/include/far_planner/dynamic_graph.h

+3-7
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,6 @@ struct DynamicGraphParams {
1313
int finalize_thred;
1414
int pool_size;
1515
int votes_size;
16-
int terrain_inflate;
17-
float traj_interval_ratio;
1816
float kConnectAngleThred;
1917
float filter_pos_margin;
2018
float filter_dirs_margin;
@@ -200,9 +198,9 @@ class DynamicGraph {
200198

201199
inline bool IsInternavInRange(const NavNodePtr& cur_inter_ptr) {
202200
if (cur_inter_ptr == NULL) return false;
203-
const float dist_thred = TRAJ_DIST;
204-
const float height_thred = FARUtil::kMarginHeight;
205-
if (cur_inter_ptr->fgscore > dist_thred || !FARUtil::IsPointInToleratedHeight(cur_inter_ptr->position, height_thred)) {
201+
if (cur_inter_ptr->fgscore > FARUtil::kLocalPlanRange ||
202+
!FARUtil::IsPointInToleratedHeight(cur_inter_ptr->position, FARUtil::kMarginHeight))
203+
{
206204
return false;
207205
}
208206
return true;
@@ -434,8 +432,6 @@ class DynamicGraph {
434432
DynamicGraph() = default;
435433
~DynamicGraph() = default;
436434

437-
static float TRAJ_DIST; // max distances between trajectory nodes
438-
439435
void Init(const ros::NodeHandle& nh, const DynamicGraphParams& params);
440436

441437
/**

‎src/far_planner/include/far_planner/far_planner.h

+2-1
Original file line numberDiff line numberDiff line change
@@ -183,7 +183,8 @@ class FARMaster {
183183
void WaypointCallBack(const geometry_msgs::PointStamped& route_goal);
184184

185185
void ExtractDynamicObsFromScan(const PointCloudPtr& scanCloudIn,
186-
const PointCloudPtr& obsCloudIn,
186+
const PointCloudPtr& obsCloudIn,
187+
const PointCloudPtr& freeCloudIn,
187188
const PointCloudPtr& dyObsCloudOut);
188189

189190
/* define inline functions */

‎src/far_planner/include/far_planner/graph_planner.h

-1
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,6 @@ struct GraphPlannerParams {
1919
int free_thred;
2020
int votes_size;
2121
int momentum_thred;
22-
int clear_inflate_size;
2322
};
2423

2524

‎src/far_planner/include/far_planner/scan_handler.h

+4-4
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,6 @@ struct ScanHandlerParams {
88
float terrain_range;
99
float voxel_size;
1010
float ceil_height;
11-
int inflate_size;
1211
};
1312

1413
enum GridStatus {
@@ -28,7 +27,7 @@ class ScanHandler {
2827

2928
void UpdateRobotPosition(const Point3D& odom_pos);
3029

31-
void SetCurrentScanCloud(const PointCloudPtr& scanCloudIn);
30+
void SetCurrentScanCloud(const PointCloudPtr& scanCloudIn, const PointCloudPtr& freeCloudIn);
3231

3332
void SetSurroundObsCloud(const PointCloudPtr& obsCloudIn,
3433
const bool& is_fiWlter_cloud=false);
@@ -52,8 +51,9 @@ class ScanHandler {
5251
int row_num_, col_num_, level_num_;
5352
bool is_grids_init_ = false;
5453
PCLPoint center_p_;
55-
56-
const float ANG_RES_Y = 2.5f/180.0f * M_PI;
54+
// Set resolution for Velodyne LiDAR PUCK: https://www.amtechs.co.jp/product/VLP-16-Puck.pdf
55+
const float ANG_RES_Y = 2.0f/180.0f * M_PI; // vertical resolution 2 degree
56+
const float ANG_RES_X = 0.5f/180.0f * M_PI; // horizontal resolution 0.5 degree
5757
std::unique_ptr<grid_ns::Grid<char>> voxel_grids_;
5858

5959
void SetMapOrigin(const Point3D& ori_robot_pos);

‎src/far_planner/include/far_planner/terrain_planner.h

-1
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,6 @@
77
struct TerrainPlannerParams {
88
TerrainPlannerParams() = default;
99
std::string world_frame;
10-
float local_range;
1110
float radius;
1211
float voxel_size;
1312
int inflate_size;

‎src/far_planner/include/far_planner/utility.h

+5-4
Original file line numberDiff line numberDiff line change
@@ -92,13 +92,15 @@ class FARUtil {
9292
static float kNewPIThred;
9393
static float kSensorRange;
9494
static float kMarginDist;
95+
static float kLocalPlanRange;
9596
static float kMarginHeight;
9697
static float kTerrainRange;
9798
static float kFreeZ;
9899
static float kObsDecayTime;
99100
static float kNewDecayTime;
100101
static int kDyObsThred;
101102
static int KNewPointC;
103+
static int kObsInflate;
102104
static float kTolerZ;
103105
static float kAcceptAlign;
104106
static float kVizRatio;
@@ -265,7 +267,7 @@ class FARUtil {
265267
static void InflateCloud(const PointCloudPtr& obsCloudInOut,
266268
const float& resol,
267269
const int& inflate_size,
268-
const bool& deep_down_inflate);
270+
const bool& deep_z_inflate);
269271

270272
static void SortEdgesClockWise(const Point3D& center, std::vector<PointPair>& edges);
271273

@@ -423,9 +425,8 @@ class FARUtil {
423425
}
424426

425427
template <typename Point>
426-
static bool IsConvexPoint(const std::vector<Point>& poly, const Point& ev_p, const Point& free_p) {
427-
const bool in_or_out = PointInsideAPoly(poly, free_p);
428-
if (!FARUtil::PointInsideAPoly(poly, ev_p) == in_or_out) {
428+
static bool IsConvexPoint(const PolygonPtr& poly_ptr, const Point& ev_p) {
429+
if (FARUtil::PointInsideAPoly(poly_ptr->vertices, ev_p) != poly_ptr->is_robot_inside) {
429430
return true;
430431
}
431432
return false;

‎src/far_planner/src/contour_graph.cpp

+8-2
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ void ContourGraph::UpdateContourGraph(const NavNodePtr& odom_node_ptr,
3232
}
3333
ContourGraph::UpdateOdomFreePosition(odom_node_ptr_, FARUtil::free_odom_p);
3434
for (const auto& poly_ptr : ContourGraph::contour_polygons_) {
35+
poly_ptr->is_robot_inside = FARUtil::PointInsideAPoly(poly_ptr->vertices, FARUtil::free_odom_p);
3536
CTNodePtr new_ctnode_ptr = NULL;
3637
if (poly_ptr->is_pillar) {
3738
Point3D mean_p = FARUtil::AveragePoints(poly_ptr->vertices);
@@ -165,9 +166,14 @@ bool ContourGraph::IsPointsConnectFreePolygon(const ConnectPair& cedge,
165166
}
166167
if (!is_global_check) {
167168
// check for local range polygons
169+
const Point3D center_p = Point3D((cedge.start_p.x + cedge.end_p.x) / 2.0f,
170+
(cedge.start_p.y + cedge.end_p.y) / 2.0f,
171+
0.0f);
168172
for (const auto& poly_ptr : ContourGraph::contour_polygons_) {
169173
if (poly_ptr->is_pillar) continue;
170-
if (ContourGraph::IsEdgeCollidePoly(poly_ptr->vertices, cedge)) {
174+
if ((poly_ptr->is_robot_inside != FARUtil::PointInsideAPoly(poly_ptr->vertices, center_p)) ||
175+
ContourGraph::IsEdgeCollidePoly(poly_ptr->vertices, cedge))
176+
{
171177
return false;
172178
}
173179
}
@@ -530,7 +536,7 @@ void ContourGraph::AnalysisConvexityOfCTNode(const CTNodePtr& ctnode_ptr) {
530536
return;
531537
}
532538
const Point3D ev_p = ctnode_ptr->position + topo_dir * FARUtil::kLeafSize;
533-
if (FARUtil::IsConvexPoint(ctnode_ptr->poly_ptr->vertices, ev_p, FARUtil::free_odom_p)) {
539+
if (FARUtil::IsConvexPoint(ctnode_ptr->poly_ptr, ev_p)) {
534540
ctnode_ptr->free_direct = NodeFreeDirect::CONVEX;
535541
} else {
536542
ctnode_ptr->free_direct = NodeFreeDirect::CONCAVE;

‎src/far_planner/src/dynamic_graph.cpp

+3-5
Original file line numberDiff line numberDiff line change
@@ -14,15 +14,13 @@ void DynamicGraph::Init(const ros::NodeHandle& nh, const DynamicGraphParams& par
1414
dg_params_ = params;
1515
CONNECT_ANGLE_COS = cos(dg_params_.kConnectAngleThred);
1616
NOISE_ANGLE_COS = cos(FARUtil::kAngleNoise);
17-
TRAJ_DIST = FARUtil::kSensorRange / dg_params_.traj_interval_ratio;
1817
id_tracker_ = 1;
1918
last_connect_pos_ = Point3D(0,0,0);
2019
/* Initialize Terrian Planner */
2120
tp_params_.world_frame = FARUtil::worldFrameId;
22-
tp_params_.local_range = TRAJ_DIST;
2321
tp_params_.voxel_size = FARUtil::kLeafSize;
2422
tp_params_.radius = FARUtil::kNearDist * 2.0f;
25-
tp_params_.inflate_size = dg_params_.terrain_inflate;
23+
tp_params_.inflate_size = FARUtil::kObsInflate;
2624
terrain_planner_.Init(nh, tp_params_);
2725
}
2826

@@ -527,7 +525,7 @@ void DynamicGraph::ReEvaluateConvexity(const NavNodePtr& node_ptr) {
527525
if (!is_wall) {
528526
const Point3D ctnode_p = node_ptr->ctnode->position;
529527
const Point3D ev_p = ctnode_p + topo_dir * FARUtil::kLeafSize;
530-
if (FARUtil::IsConvexPoint(node_ptr->ctnode->poly_ptr->vertices, ev_p, FARUtil::free_odom_p)) {
528+
if (FARUtil::IsConvexPoint(node_ptr->ctnode->poly_ptr, ev_p)) {
531529
node_ptr->free_direct = NodeFreeDirect::CONVEX;
532530
} else {
533531
node_ptr->free_direct = NodeFreeDirect::CONCAVE;
@@ -769,7 +767,7 @@ void DynamicGraph::UpdateGlobalNearNodes() {
769767
if (node_ptr->is_navpoint) {
770768
node_ptr->position.intensity = node_ptr->fgscore;
771769
internav_near_nodes_.push_back(node_ptr);
772-
if ((node_ptr->position - odom_node_ptr_->position).norm() < TRAJ_DIST / 2.0f) {
770+
if ((node_ptr->position - odom_node_ptr_->position).norm() < FARUtil::kLocalPlanRange / 2.0f) {
773771
surround_internav_nodes_.push_back(node_ptr);
774772
}
775773
}

0 commit comments

Comments
 (0)