fix todos on px4 parameters and ros parameters, lower obstacle distance

cost such that it's possible to fly at lower altitudes
PX4 · Aug 29, 2019 · 5628fa2 · 5628fa2
1 parent bfed37c
commit 5628fa2
Show file tree

Hide file tree

Showing 6 changed files with 10 additions and 5 deletions.
diff --git a/avoidance/src/avoidance_node.cpp b/avoidance/src/avoidance_node.cpp
@@ -134,6 +134,10 @@ void AvoidanceNode::checkPx4Parameters() {
     request_param("MPC_COL_PREV_D", px4_.param_mpc_col_prev_d);
     request_param("MPC_LAND_SPEED", px4_.param_mpc_land_speed);
     request_param("NAV_ACC_RAD", px4_.param_nav_acc_rad);
+    request_param("MPC_Z_VEL_MAX_DN", px4_.param_mpc_z_vel_max_dn);
+    request_param("MPC_Z_VEL_MAX_UP", px4_.param_mpc_z_vel_max_up);
+    request_param("MPC_ACC_HOR", px4_.param_mpc_acc_hor);
+    request_param("MPC_JERK_MAX", px4_.param_mpc_jerk_max);
 
     if (!std::isfinite(px4_.param_mpc_xy_cruise) || !std::isfinite(px4_.param_mpc_col_prev_d) ||
         !std::isfinite(px4_.param_mpc_land_speed) || !std::isfinite(px4_.param_nav_acc_rad)) {

diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg
@@ -12,8 +12,9 @@ gen.add("min_sensor_range_", double_t, 0, "Discard points closer than that", 0.2
 gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 25.0, 0, 30.0)
 gen.add("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 3.0, 0, 20.0)
 gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 6000, 0.0, 50000.0)
-gen.add("obstacle_cost_param_", double_t, 0,"Approximate distance from obstacles (m) when the obstacle distance term dominates the cost function", 8.5, 0.0, 30.0)
+gen.add("obstacle_cost_param_", double_t, 0,"Approximate distance from obstacles (m) when the obstacle distance term dominates the cost function", 5.0, 0.0, 30.0)
 gen.add("tree_heuristic_weight_", double_t, 0, "Weight for the tree heuristic cost", 35.0, 0.0, 50.0)
+gen.add("tree_step_size_s_", double_t, 0, "Tree time step size in seconds", 0.05, 0.01, 5.0)
 gen.add("goal_z_param", double_t, 0, "Height of the goal position", 3.5, -20.0, 20.0)
 gen.add("timeout_startup_", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 5, 0, 60)
 gen.add("timeout_critical_", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 0.5, 0, 10)

diff --git a/local_planner/cfg/vehicle.yaml b/local_planner/cfg/vehicle.yaml
@@ -37,7 +37,7 @@ dictitems:
   min_num_points_per_cell_: 1
   min_sensor_range_: 0.2
   n_expanded_nodes_: 40
-  obstacle_cost_param_: 8.5
+  obstacle_cost_param_: 5.0
   pitch_cost_param_: 25.0
   max_sensor_range_: 15.0
   smoothing_margin_degrees_: 40.0

diff --git a/local_planner/include/local_planner/star_planner.h b/local_planner/include/local_planner/star_planner.h
@@ -28,6 +28,7 @@ class StarPlanner {
   float smoothing_margin_degrees_ = 40.f;
   float tree_heuristic_weight_ = 35.f;
   float acceptance_radius_ = 2.f;
+  float tree_step_size_s_ = 0.05f;
 
   pcl::PointCloud<pcl::PointXYZI> cloud_;
 

diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp
@@ -126,7 +126,6 @@ void LocalPlanner::determineStrategy() {
                     cost_matrix_, cost_image_data_);
 
       simulation_limits lims;
-      setDefaultPx4Parameters();  // TODO: remove but make sure they're set!
       lims.max_z_velocity = px4_.param_mpc_z_vel_max_up;
       lims.min_z_velocity = -1.0f * px4_.param_mpc_z_vel_max_dn;
       lims.max_xy_velocity_norm = px4_.param_mpc_xy_cruise;

diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp
@@ -18,6 +18,7 @@ void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerN
   max_path_length_ = static_cast<float>(config.max_sensor_range_);
   smoothing_margin_degrees_ = static_cast<float>(config.smoothing_margin_degrees_);
   tree_heuristic_weight_ = static_cast<float>(config.tree_heuristic_weight_);
+  tree_step_size_s_ = static_cast<float>(config.tree_step_size_s_);
 }
 
 void StarPlanner::setParams(const costParameters& cost_params, const simulation_limits& limits, float acc_rad) {
@@ -41,7 +42,6 @@ float StarPlanner::treeHeuristicFunction(int node_number) const {
 
 void StarPlanner::buildLookAheadTree() {
   std::clock_t start_time = std::clock();
-
   Histogram histogram(ALPHA_RES);
   std::vector<uint8_t> cost_image_data;
   std::vector<candidateDirection> candidate_vector;
@@ -87,7 +87,7 @@ void StarPlanner::buildLookAheadTree() {
       int children = 0;
       for (candidateDirection candidate : candidate_vector) {
         simulation_state state = tree_[origin].state;
-        TrajectorySimulator sim(lims_, state, 0.05f);  // todo: parameterize simulation step size [s]
+        TrajectorySimulator sim(lims_, state, tree_step_size_s_);
         std::vector<simulation_state> trajectory = sim.generate_trajectory(candidate.toEigen(), tree_node_duration_);
 
         // check if another close node has been added