Plan dynamically feasible trajectories

avoidance/include/avoidance/common.h

@@ -85,7 +85,7 @@ struct ModelParameters {
   float param_acc_up_max = NAN;   // Maximum vertical acceleration in velocity controlled modes upward
   float param_mpc_z_vel_max_up = NAN;   // Maximum vertical ascent velocity
   float param_mpc_acc_down_max = NAN; // Maximum vertical acceleration in velocity controlled modes down
-  float param_mpc_vel_max_dn = NAN; // Maximum vertical descent velocity
+  float param_mpc_z_vel_max_dn = NAN; // Maximum vertical descent velocity
   float param_mpc_acc_hor = NAN;  // Maximum horizontal acceleration for auto mode and
                       // maximum deceleration for manual mode
   float param_mpc_xy_cruise = NAN;   // Desired horizontal velocity in mission
@@ -352,6 +352,27 @@ pcl::PointCloud<pcl::PointXYZ> removeNaNAndGetMaxima(pcl::PointCloud<pcl::PointX
 *                  bigger FOV than previously thought
 **/
 void updateFOVFromMaxima(FOV& fov, const pcl::PointCloud<pcl::PointXYZ>& maxima);
+/**
+* @brief     compute the maximum speed allowed based on sensor range and vehicle tuning
+* @param     jerk, vehicle maximum jerk
+* @param     accel, vehicle maximum horizontal acceleration
+* @param     braking_distance, maximum sensor range
+* @returns   maximum speed
+**/
+float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance);
+
+/**
+* @brief     compute if the cruise speed requested by paramters or mission item is feasible based on vehicle dynamics
+*and sesnor range
+* @param     jerk, vehicle maximum jerk
+* @param     accel, vehicle maximum horizontal acceleration
+* @param     braking_distance, maximum sensor range
+* @param     mpc_xy_cruise, desired speed set from parameter
+* @param     mission_item_speed, desired speed set from mission item
+* @returns   maximum speed
+**/
+float getMaxSpeed(const float jerk, const float accel, const float braking_distance, const float mpc_xy_cruise,
+                  const float mission_item_speed);
 
 inline Eigen::Vector3f toEigen(const geometry_msgs::Point& p) {
   Eigen::Vector3f ev3(p.x, p.y, p.z);

avoidance/src/avoidance_node.cpp

@@ -113,7 +113,7 @@ void AvoidanceNode::px4ParamsCallback(const mavros_msgs::Param& msg) {
   parse_param_f("MPC_LAND_SPEED", px4_.param_mpc_land_speed) ||
   parse_param_f("MPC_TKO_SPEED", px4_.param_mpc_tko_speed) ||
   parse_param_f("MPC_XY_CRUISE", px4_.param_mpc_xy_cruise) ||
-  parse_param_f("MPC_Z_VEL_MAX_DN", px4_.param_mpc_vel_max_dn) ||
+  parse_param_f("MPC_Z_VEL_MAX_DN", px4_.param_mpc_z_vel_max_dn) ||
   parse_param_f("MPC_Z_VEL_MAX_UP", px4_.param_mpc_z_vel_max_up) ||
   parse_param_f("MPC_COL_PREV_D", px4_.param_mpc_col_prev_d) ||
   parse_param_f("NAV_ACC_RAD", px4_.param_nav_acc_rad);
@@ -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)) {

avoidance/src/common.cpp

@@ -418,4 +418,22 @@ void updateFOVFromMaxima(FOV& fov, const pcl::PointCloud<pcl::PointXYZ>& maxima)
   }
 }
 
+float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance) {
+  // Calculate maximum speed given the sensor range and vehicle parameters
+  // We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from
+  // opposite max acceleration)
+  // Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk)
+  float b = 4.f * accel * accel / jerk;
+  float c = -2.f * accel * braking_distance;
+
+  return 0.5f * (-b + sqrtf(b * b - 4.f * c));
+}
+
+float getMaxSpeed(const float jerk, const float accel, const float braking_distance, const float mpc_xy_cruise,
+                  const float mission_item_speed) {
+  float limited_speed = computeMaxSpeedFromBrakingDistance(jerk, accel, braking_distance);
+  float speed = std::isfinite(mission_item_speed) ? mission_item_speed : mpc_xy_cruise;
+  return std::min(speed, limited_speed);
+}
+
 }  // namespace avoidance

local_planner/cfg/local_planner_node.cfg

@@ -9,11 +9,12 @@ gen = ParameterGenerator()
 
 gen.add("max_sensor_range_",    double_t,    0, "Data points farther away will be discarded", 15.0,  0, 40)
 gen.add("min_sensor_range_", double_t, 0, "Discard points closer than that", 0.2, 0, 10)
-gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 25.0, 0, 30.0)
+gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 50.0, 0, 100.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("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 3000, 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", 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)
@@ -27,6 +28,6 @@ gen.add("smoothing_margin_degrees_", double_t, 0, "smoothing radius for obstacle
 # star_planner
 gen.add("children_per_node_",    int_t,    0, "Branching factor of the search tree", 8,  0, 100)
 gen.add("n_expanded_nodes_",    int_t,    0, "Number of nodes expanded in complete tree", 40,  0, 200)
-gen.add("tree_node_distance_",    double_t,    0, "Distance between nodes", 2,  0, 20)
+gen.add("tree_node_duration_",    double_t,    0, "Distance in seconds between nodes", 0.5,  0.01, 2)
 
 exit(gen.generate(PACKAGE, "avoidance", "LocalPlannerNode"))

local_planner/cfg/vehicle.yaml

@@ -1,54 +1,53 @@
-# This is a sample config file for a real vehicle
 !!python/object/new:dynamic_reconfigure.encoding.Config
 dictitems:
-  adapt_cost_params_: true
-  box_radius_: 12.0
-  children_per_node_: 50
-  goal_cost_param_: 10.0
+  children_per_node_: 8
   goal_z_param: 3.0
   groups: !!python/object/new:dynamic_reconfigure.encoding.Config
     dictitems:
-      adapt_cost_params_: true
-      box_radius_: 12.0
-      children_per_node_: 50
-      goal_cost_param_: 10.0
+      children_per_node_: 8
       goal_z_param: 3.0
       groups: !!python/object/new:dynamic_reconfigure.encoding.Config
         state: []
-      heading_cost_param_: 0.5
       id: 0
       max_point_age_s_: 20.0
-      min_num_points_per_cell_: 25 # Tune to your sensor requirements!
+      min_num_points_per_cell_: 1
       min_sensor_range_: 0.2
-      n_expanded_nodes_: 10
+      n_expanded_nodes_: 40
       name: Default
-      no_progress_slope_: -0.0007
+      obstacle_cost_param_: 8.5
       parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
         state: []
       parent: 0
-      smooth_cost_param_: 1.5
+      pitch_cost_param_: 25.0
+      sensor_range_: 15.0
       smoothing_margin_degrees_: 40.0
       smoothing_speed_xy_: 10.0
       smoothing_speed_z_: 3.0
       state: true
       timeout_critical_: 0.5
+      timeout_startup_: 5.0
       timeout_termination_: 15.0
-      tree_discount_factor_: 0.8
-      tree_node_distance_: 1.0
+      tree_heuristic_weight_: 35.0
+      tree_node_duration_: 0.5
       type: ''
+      velocity_cost_param_: 6000.0
+      yaw_cost_param_: 3.0
     state: []
-  heading_cost_param_: 0.5
   max_point_age_s_: 20.0
-  min_num_points_per_cell_: 25 # Tune to your sensor requirements!
+  min_num_points_per_cell_: 1
   min_sensor_range_: 0.2
-  n_expanded_nodes_: 10
-  no_progress_slope_: -0.0007
-  smooth_cost_param_: 1.5
+  n_expanded_nodes_: 40
+  obstacle_cost_param_: 5.0
+  pitch_cost_param_: 25.0
+  max_sensor_range_: 15.0
   smoothing_margin_degrees_: 40.0
   smoothing_speed_xy_: 10.0
   smoothing_speed_z_: 3.0
   timeout_critical_: 0.5
+  timeout_startup_: 5.0
   timeout_termination_: 15.0
-  tree_discount_factor_: 0.8
-  tree_node_distance_: 1.0
+  tree_heuristic_weight_: 35.0
+  tree_node_duration_: 0.5
+  velocity_cost_param_: 6000.0
+  yaw_cost_param_: 3.0
 state: []

local_planner/include/local_planner/avoidance_output.h

@@ -11,16 +11,13 @@ enum waypoint_choice { hover, tryPath, direct, reachHeight };
 
 struct avoidanceOutput {
   waypoint_choice waypoint_type = hover;
-  bool obstacle_ahead;       // true is there is an obstacle ahead of the vehicle
-  float cruise_velocity;     // mission cruise velocity
+  bool obstacle_ahead;    // true is there is an obstacle ahead of the vehicle
+  float cruise_velocity;  // mission cruise velocity
+  float tree_node_duration;
   ros::Time last_path_time;  // finish built time for the VFH+* tree
 
   Eigen::Vector3f take_off_pose;  // last vehicle position when not armed
 
-  std::vector<Eigen::Vector3f> path_node_positions;  // array of tree nodes
-                                                     // position, each node
-                                                     // is the minimum cost
-                                                     // node for each tree
-                                                     // depth level
+  std::vector<Eigen::Vector3f> path_node_setpoints;  // array of setpoints
 };
 }

local_planner/include/local_planner/candidate_direction.h

@@ -1,19 +1,36 @@
 #pragma once
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <cmath>
 #include "avoidance/common.h"
+#include "local_planner/tree_node.h"
 
 namespace avoidance {
 
 struct candidateDirection {
   float cost;
   float elevation_angle;
   float azimuth_angle;
+  TreeNode tree_node;
 
-  candidateDirection(float c, float e, float z) : cost(c), elevation_angle(e), azimuth_angle(z){};
+  candidateDirection(float c, float e, float z) : cost(c), elevation_angle(e), azimuth_angle(z) {
+    simulation_state start_state;
+    start_state.position = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
+    start_state.velocity = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
+    start_state.acceleration = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
+    start_state.time = ros::Time::now().toSec();
+    tree_node = TreeNode(0, start_state, Eigen::Vector3f::Zero());
+  };
 
   bool operator<(const candidateDirection& y) const { return cost < y.cost; }
 
   bool operator>(const candidateDirection& y) const { return cost > y.cost; }
 
   PolarPoint toPolar(float r) const { return PolarPoint(elevation_angle, azimuth_angle, r); }
+  Eigen::Vector3f toEigen() const {
+    return Eigen::Vector3f(std::cos(elevation_angle * DEG_TO_RAD) * std::sin(azimuth_angle * DEG_TO_RAD),
+                           std::cos(elevation_angle * DEG_TO_RAD) * std::cos(azimuth_angle * DEG_TO_RAD),
+                           std::sin(elevation_angle * DEG_TO_RAD));
+  }
 };
 }

local_planner/include/local_planner/local_planner.h

@@ -7,6 +7,7 @@
 #include "candidate_direction.h"
 #include "cost_parameters.h"
 #include "planner_functions.h"
+#include "trajectory_simulator.h"
 
 #include <dynamic_reconfigure/server.h>
 #include <local_planner/LocalPlannerNodeConfig.h>
@@ -44,6 +45,7 @@ class LocalPlanner {
   float max_point_age_s_ = 10;
   float yaw_fcu_frame_deg_ = 0.0f;
   float pitch_fcu_frame_deg_ = 0.0f;
+  float tree_node_duration_ = 0.5f;
 
   std::vector<FOV> fov_fcu_frame_;
 
@@ -174,11 +176,11 @@ class LocalPlanner {
   /**
   * @brief     getter method to visualize the tree in rviz
   * @param[in] tree, the whole tree built during planning (vector of nodes)
-  * @param[in] closed_set, velocity message coming from the FCU
-  * @param[in] path_node_positions, velocity message coming from the FCU
+  * @param[in] closed_set
+  * @param[in] path_node_setpoints
   **/
   void getTree(std::vector<TreeNode>& tree, std::vector<int>& closed_set,
-               std::vector<Eigen::Vector3f>& path_node_positions) const;
+               std::vector<Eigen::Vector3f>& path_node_setpoints) const;
   /**
   * @brief     getter method for obstacle distance information
   * @param     obstacle_distance, obstacle distance message to fill

local_planner/include/local_planner/local_planner_visualization.h

@@ -36,11 +36,11 @@ class LocalPlannerVisualization {
   *              chosen
   * @params[in]  tree, the complete calculated search tree
   * @params[in]  closed_set, the closed set (all expanded nodes)
-  * @params[in]  path_node_positions, the positions of all nodes belonging to
+  * @params[in]  path_node_setpoints, the setpoints of all nodes belonging to
   *              the chosen best path
   **/
   void publishTree(const std::vector<TreeNode>& tree, const std::vector<int>& closed_set,
-                   const std::vector<Eigen::Vector3f>& path_node_positions) const;
+                   const std::vector<Eigen::Vector3f>& path_node_setpoints) const;
 
   /**
   * @brief       Visualization of the goal position
@@ -113,6 +113,7 @@ class LocalPlannerVisualization {
   void publishFOV(const std::vector<FOV>& fov, float max_range) const;
 
   void publishRangeScan(const sensor_msgs::LaserScan& scan, const geometry_msgs::PoseStamped& newest_pose) const;
+  std::tuple<float, float, float> HSVtoRGB(std::tuple<float, float, float> hsv) const;
 
  private:
   ros::Publisher local_pointcloud_pub_;
@@ -135,6 +136,7 @@ class LocalPlannerVisualization {
   ros::Publisher deg60_point_pub_;
   ros::Publisher fov_pub_;
   ros::Publisher range_scan_pub_;
+  ros::Publisher tree_cost_pub_;
 
   int path_length_ = 0;
 };

local_planner/include/local_planner/planner_functions.h

@@ -143,11 +143,12 @@ void printHistogram(const Histogram& histogram);
 * @brief      Returns a setpoint that lies on the given path
 * @param[in]  vector of nodes defining the path, with the last node of the path at index 0
 * @param[in]  ros time of path generation
-* @param[in]  velocity, scalar value for the norm of the current vehicle velocity
+* @param[in]  tree_node_duration, scalar value for length in time each path segment is to be used
 * @param[out] setpoint on the tree toward which the drone should fly
 * @returns    boolean indicating whether the tree was valid
 **/
-bool getSetpointFromPath(const std::vector<Eigen::Vector3f>& path, const ros::Time& path_generation_time,
-                         float velocity, Eigen::Vector3f& setpoint);
+bool interpolateBetweenSetpoints(const std::vector<Eigen::Vector3f>& setpoint_array,
+                                 const ros::Time& path_generation_time, float tree_node_duration,
+                                 Eigen::Vector3f& setpoint);
 }
 #endif  // LOCAL_PLANNER_FUNCTIONS_H

local_planner/include/local_planner/star_planner.h

@@ -3,6 +3,7 @@
 
 #include "avoidance/histogram.h"
 #include "cost_parameters.h"
+#include "trajectory_simulator.h"
 
 #include <Eigen/Dense>
 
@@ -14,25 +15,42 @@
 #include <dynamic_reconfigure/server.h>
 #include <local_planner/LocalPlannerNodeConfig.h>
 
+#include <queue>
 #include <vector>
 
 namespace avoidance {
+
+template <typename T, typename Container = std::deque<T>, typename Compare = std::less<typename Container::value_type> >
+class iterable_priority_queue : public std::priority_queue<T, Container, Compare> {
+ public:
+  typedef typename Container::iterator iterator;
+  typedef typename Container::const_iterator const_iterator;
+
+  iterator begin() { return this->c.begin(); }
+  iterator end() { return this->c.end(); }
+  const_iterator begin() const { return this->c.begin(); }
+  const_iterator end() const { return this->c.end(); }
+};
+
 class TreeNode;
 
 class StarPlanner {
-  int children_per_node_ = 1;
-  int n_expanded_nodes_ = 5;
-  float tree_node_distance_ = 1.0f;
-  float max_path_length_ = 4.f;
-  float smoothing_margin_degrees_ = 30.f;
-  float tree_heuristic_weight_ = 10.0f;
+  int children_per_node_ = 8;
+  int n_expanded_nodes_ = 40;
+  float tree_node_duration_ = 0.5f;
+  float max_path_length_ = 15.f;
+  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_;
 
   Eigen::Vector3f goal_ = Eigen::Vector3f(NAN, NAN, NAN);
   Eigen::Vector3f position_ = Eigen::Vector3f(NAN, NAN, NAN);
   Eigen::Vector3f velocity_ = Eigen::Vector3f(NAN, NAN, NAN);
   costParameters cost_params_;
+  simulation_limits lims_;
 
  protected:
   /**
@@ -43,7 +61,7 @@ class StarPlanner {
   float treeHeuristicFunction(int node_number) const;
 
  public:
-  std::vector<Eigen::Vector3f> path_node_positions_;
+  std::vector<Eigen::Vector3f> path_node_setpoints_;
   std::vector<int> closed_set_;
   std::vector<TreeNode> tree_;
 
@@ -53,8 +71,9 @@ class StarPlanner {
   /**
   * @brief     setter method for costMatrix paramters
   * @param[in] cost_params, parameters for the histogram cost function
+  * @param[in] simulation limits defining maximum acceleration, velocity, and jerk
   **/
-  void setParams(costParameters cost_params);
+  void setParams(const costParameters& cost_params, const simulation_limits& limits, float acc_rad);
 
   /**
   * @brief     setter method for star_planner pointcloud