Skip to content

Commit

Permalink
planner_cspace: enable dynamic reconfigure (#714)
Browse files Browse the repository at this point in the history
  • Loading branch information
@nhatao
nhatao authored Aug 30, 2023
1 parent 3c622ab commit 0f7e331
Show file tree
Hide file tree
Showing 7 changed files with 315 additions and 25 deletions.
8 changes: 7 additions & 1 deletion planner_cspace/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ set(CATKIN_DEPENDS

actionlib
diagnostic_updater
dynamic_reconfigure
geometry_msgs
move_base_msgs
nav_msgs
Expand All @@ -27,6 +28,11 @@ set(CATKIN_DEPENDS
find_package(catkin REQUIRED COMPONENTS ${CATKIN_DEPENDS})
find_package(Boost REQUIRED COMPONENTS chrono)
find_package(OpenMP REQUIRED)

generate_dynamic_reconfigure_options(
cfg/Planner3D.cfg
)

catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS ${CATKIN_DEPENDS}
Expand All @@ -51,7 +57,7 @@ add_executable(planner_3d
src/rotation_cache.cpp
)
target_link_libraries(planner_3d ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenMP_CXX_FLAGS})
add_dependencies(planner_3d ${catkin_EXPORTED_TARGETS})
add_dependencies(planner_3d ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
set_property(TARGET planner_3d PROPERTY PUBLIC_HEADER
include/planner_cspace/bbf.h
include/planner_cspace/blockmem_gridmap.h
Expand Down
48 changes: 48 additions & 0 deletions planner_cspace/cfg/Planner3D.cfg
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
#!/usr/bin/env python
PACKAGE = "planner_cspace"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("freq", double_t, 0, "", 4.0, 0.0, 100.0)
gen.add("freq_min", double_t, 0, "", 2.0, 0.0, 100.0)
gen.add("search_timeout_abort", double_t, 0, "", 30.0, 0.0, 100.0)
gen.add("search_range", double_t, 0, "", 0.4, 0.0, 100.0)
gen.add("antialias_start", bool_t, 0, "", False)
gen.add("costmap_watchdog", double_t, 0, "", 0.0, 0.0, 100.0)
gen.add("max_vel", double_t, 0, "", 0.3, 0.0, 100.0)
gen.add("max_ang_vel", double_t, 0, "", 0.6, 0.0, 100.0)
gen.add("min_curve_radius", double_t, 0, "", 0.1, 0.0, 100.0)
gen.add("weight_decel", double_t, 0, "", 50.0, 0.0, 1000.0)
gen.add("weight_backward", double_t, 0, "", 0.9, 0.0, 1000.0)
gen.add("weight_ang_vel", double_t, 0, "", 1.0, 0.0, 1000.0)
gen.add("weight_costmap", double_t, 0, "", 50.0, 0.0, 1000.0)
gen.add("weight_costmap_turn", double_t, 0, "", 0.0, 0.0, 1000.0)
gen.add("weight_remembered", double_t, 0, "", 1000.0, 0.0, 1000.0)
gen.add("cost_in_place_turn", double_t, 0, "", 30.0, 0.0, 1000.0)
gen.add("hysteresis_max_dist", double_t, 0, "", 0.1, 0.0, 10.0)
gen.add("hysteresis_expand", double_t, 0, "", 0.1, 0.0, 10.0)
gen.add("weight_hysteresis", double_t, 0, "", 5.0, 0.0, 1000.0)
gen.add("goal_tolerance_lin", double_t, 0, "", 0.05, 0.0, 10.0)
gen.add("goal_tolerance_ang", double_t, 0, "", 0.1, 0.0, 3.14159265359)
gen.add("goal_tolerance_ang_finish", double_t, 0, "", 0.05, 0.0, 3.14159265359)
gen.add("overwrite_cost", bool_t, 0, "", False)
gen.add("hist_ignore_range", double_t, 0, "", 0.6, 0.0, 100.0)
gen.add("hist_ignore_range_max", double_t, 0, "", 1.25, 0.0, 100.0)
gen.add("remember_updates", bool_t, 0, "", False)
gen.add("remember_hit_prob", double_t, 0, "", 0.6, 0.0, 1.0)
gen.add("remember_miss_prob", double_t, 0, "", 0.3, 0.0, 1.0)
gen.add("local_range", double_t, 0, "", 2.5, 0.0, 100.0)
gen.add("longcut_range", double_t, 0, "", 0.0, 0.0, 100.0)
gen.add("esc_range", double_t, 0, "", 0.25, 0.0, 100.0)
gen.add("tolerance_range", double_t, 0, "", 0.25, 0.0, 1.0)
gen.add("tolerance_angle", double_t, 0, "", 0.0, 0.0, 3.14159265359)
gen.add("sw_wait", double_t, 0, "", 2.0, 0.0, 100.0)
gen.add("find_best", bool_t, 0, "", True)
gen.add("force_goal_orientation", bool_t, 0, "", True)
gen.add("temporary_escape", bool_t, 0, "", True)
gen.add("fast_map_update", bool_t, 0, "", False)
gen.add("max_retry_num", int_t, 0, "", -1, -1, 100)

exit(gen.generate(PACKAGE, "planner_cspace", "Planner3D"))
1 change: 1 addition & 0 deletions planner_cspace/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

<depend>actionlib</depend>
<depend>diagnostic_updater</depend>
<depend>dynamic_reconfigure</depend>
<depend>geometry_msgs</depend>
<depend>move_base_msgs</depend>
<depend>nav_msgs</depend>
Expand Down
128 changes: 104 additions & 24 deletions planner_cspace/src/planner_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
#include <costmap_cspace_msgs/CSpace3D.h>
#include <costmap_cspace_msgs/CSpace3DUpdate.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <dynamic_reconfigure/server.h>
#include <geometry_msgs/PoseArray.h>
#include <nav_msgs/GetPlan.h>
#include <nav_msgs/OccupancyGrid.h>
Expand All @@ -67,6 +68,7 @@

#include <neonavigation_common/compatibility.h>

#include <planner_cspace/Planner3DConfig.h>
#include <planner_cspace/bbf.h>
#include <planner_cspace/grid_astar.h>
#include <planner_cspace/jump_detector.h>
Expand Down Expand Up @@ -114,6 +116,7 @@ class Planner3dNode
planner_cspace_msgs::MoveWithToleranceGoalConstPtr goal_tolerant_;
tf2_ros::Buffer tfbuf_;
tf2_ros::TransformListener tfl_;
dynamic_reconfigure::Server<Planner3DConfig> parameter_server_;

Astar as_;
Astar::Gridmap<char, 0x40> cm_;
Expand Down Expand Up @@ -167,6 +170,7 @@ class Planner3dNode
float remember_miss_odds_;
bool use_path_with_velocity_;
bool retain_last_error_status_;
int num_cost_estim_task_;

JumpDetector jump_;
std::string robot_frame_;
Expand Down Expand Up @@ -928,27 +932,7 @@ class Planner3dNode
map_info_.angular_resolution != msg->info.angular_resolution)
{
map_info_ = msg->info;

range_ = static_cast<int>(search_range_ / map_info_.linear_resolution);
hist_ignore_range_ = std::lround(hist_ignore_range_f_ / map_info_.linear_resolution);
hist_ignore_range_max_ = std::lround(hist_ignore_range_max_f_ / map_info_.linear_resolution);
local_range_ = std::lround(local_range_f_ / map_info_.linear_resolution);
esc_range_ = std::lround(esc_range_f_ / map_info_.linear_resolution);
esc_angle_ = map_info_.angle / 8;
tolerance_range_ = std::lround(tolerance_range_f_ / map_info_.linear_resolution);
tolerance_angle_ = std::lround(tolerance_angle_f_ / map_info_.angular_resolution);
goal_tolerance_lin_ = std::lround(goal_tolerance_lin_f_ / map_info_.linear_resolution);
goal_tolerance_ang_ = std::lround(goal_tolerance_ang_f_ / map_info_.angular_resolution);
cc_.angle_resolution_aspect_ = 2.0 / tanf(map_info_.angular_resolution);

model_.reset(
new GridAstarModel3D(
map_info_,
ec_,
local_range_,
cost_estim_cache_.gridmap(), cm_, cm_hyst_, cm_rough_,
cc_, range_));

resetGridAstarModel();
ROS_DEBUG("Search model updated");
}
else
Expand Down Expand Up @@ -1092,6 +1076,7 @@ class Planner3dNode
: nh_()
, pnh_("~")
, tfl_(tfbuf_)
, parameter_server_(pnh_)
, cost_estim_cache_(cm_rough_, bbf_costmap_)
, jump_(tfbuf_)
{
Expand Down Expand Up @@ -1242,9 +1227,8 @@ class Planner3dNode
int num_task;
pnh_.param("num_search_task", num_task, num_threads * 16);
as_.setSearchTaskNum(num_task);
int num_cost_estim_task;
pnh_.param("num_cost_estim_task", num_cost_estim_task, num_threads * 16);
cost_estim_cache_.setParams(cc_, num_cost_estim_task);
pnh_.param("num_cost_estim_task", num_cost_estim_task_, num_threads * 16);
cost_estim_cache_.setParams(cc_, num_cost_estim_task_);

pnh_.param("retain_last_error_status", retain_last_error_status_, true);
status_.status = planner_cspace_msgs::PlannerStatus::DONE;
Expand All @@ -1263,6 +1247,102 @@ class Planner3dNode

act_->start();
act_tolerant_->start();

// cbParameter() with the inital parameters will be called within setCallback().
parameter_server_.setCallback(boost::bind(&Planner3dNode::cbParameter, this, _1, _2));
}

void resetGridAstarModel()
{
range_ = static_cast<int>(search_range_ / map_info_.linear_resolution);
hist_ignore_range_ = std::lround(hist_ignore_range_f_ / map_info_.linear_resolution);
hist_ignore_range_max_ = std::lround(hist_ignore_range_max_f_ / map_info_.linear_resolution);
local_range_ = std::lround(local_range_f_ / map_info_.linear_resolution);
esc_range_ = std::lround(esc_range_f_ / map_info_.linear_resolution);
esc_angle_ = map_info_.angle / 8;
tolerance_range_ = std::lround(tolerance_range_f_ / map_info_.linear_resolution);
tolerance_angle_ = std::lround(tolerance_angle_f_ / map_info_.angular_resolution);
goal_tolerance_lin_ = std::lround(goal_tolerance_lin_f_ / map_info_.linear_resolution);
goal_tolerance_ang_ = std::lround(goal_tolerance_ang_f_ / map_info_.angular_resolution);
cc_.angle_resolution_aspect_ = 2.0 / tanf(map_info_.angular_resolution);

model_.reset(
new GridAstarModel3D(
map_info_,
ec_,
local_range_,
cost_estim_cache_.gridmap(), cm_, cm_hyst_, cm_rough_,
cc_, range_));
}

void cbParameter(const Planner3DConfig& config, const uint32_t /* level */)
{
freq_ = config.freq;
freq_min_ = config.freq_min;
search_timeout_abort_ = config.search_timeout_abort;
search_range_ = config.search_range;
antialias_start_ = config.antialias_start;
costmap_watchdog_ = ros::Duration(config.costmap_watchdog);

cc_.max_vel_ = config.max_vel;
cc_.max_ang_vel_ = config.max_ang_vel;
cc_.min_curve_radius_ = config.min_curve_radius;
cc_.weight_decel_ = config.weight_decel;
cc_.weight_backward_ = config.weight_backward;
cc_.weight_ang_vel_ = config.weight_ang_vel;
cc_.weight_costmap_ = config.weight_costmap;
cc_.weight_costmap_turn_ = config.weight_costmap_turn;
cc_.weight_remembered_ = config.weight_remembered;
cc_.in_place_turn_ = config.cost_in_place_turn;
cc_.hysteresis_max_dist_ = config.hysteresis_max_dist;
cc_.hysteresis_expand_ = config.hysteresis_expand;
cc_.weight_hysteresis_ = config.weight_hysteresis;

goal_tolerance_lin_f_ = config.goal_tolerance_lin;
goal_tolerance_ang_f_ = config.goal_tolerance_ang;
goal_tolerance_ang_finish_ = config.goal_tolerance_ang_finish;

overwrite_cost_ = config.overwrite_cost;
hist_ignore_range_f_ = config.hist_ignore_range;
hist_ignore_range_max_f_ = config.hist_ignore_range_max;

remember_updates_ = config.remember_updates;
remember_hit_odds_ = bbf::probabilityToOdds(config.remember_hit_prob);
remember_miss_odds_ = bbf::probabilityToOdds(config.remember_miss_prob);

local_range_f_ = config.local_range;
longcut_range_f_ = config.longcut_range;
esc_range_f_ = config.esc_range;
tolerance_range_f_ = config.tolerance_range;
tolerance_angle_f_ = config.tolerance_angle;
find_best_ = config.find_best;
force_goal_orientation_ = config.force_goal_orientation;
temporary_escape_ = config.temporary_escape;
fast_map_update_ = config.fast_map_update;
max_retry_num_ = config.max_retry_num;
sw_wait_ = config.sw_wait;

cost_estim_cache_.setParams(cc_, num_cost_estim_task_);
ec_ = Astar::Vecf(
1.0f / cc_.max_vel_,
1.0f / cc_.max_vel_,
1.0f * cc_.weight_ang_vel_ / cc_.max_ang_vel_);

if (map_info_.linear_resolution != 0.0 && map_info_.angular_resolution != 0.0)
{
resetGridAstarModel();
const Astar::Vec size2d(static_cast<int>(map_info_.width), static_cast<int>(map_info_.height), 1);
const DistanceMap::Params dmp =
{
.euclid_cost = ec_,
.range = range_,
.local_range = local_range_,
.longcut_range = static_cast<int>(std::lround(longcut_range_f_ / map_info_.linear_resolution)),
.size = size2d,
.resolution = map_info_.linear_resolution,
};
cost_estim_cache_.init(model_, dmp);
}
}

GridAstarModel3D::Vec pathPose2Grid(const geometry_msgs::PoseStamped& pose) const
Expand Down
6 changes: 6 additions & 0 deletions planner_cspace/test/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,6 +149,12 @@ add_rostest_gtest(test_tolerant_action
target_link_libraries(test_tolerant_action ${catkin_LIBRARIES})
add_dependencies(test_tolerant_action planner_3d)

add_rostest_gtest(test_dynamic_parameter_change
test/dynamic_parameter_change_rostest.test
src/test_dynamic_parameter_change.cpp
)
target_link_libraries(test_dynamic_parameter_change ${catkin_LIBRARIES})
add_dependencies(test_dynamic_parameter_change planner_3d)

if(NEONAVIGATION_EXTRA_TESTS)

Expand Down
Loading

0 comments on commit 0f7e331

Please sign in to comment.