Simplify cost functions (#443)

This commit merges the two cost functions into one, getting rid of a bunch
of over-designed logic.

avoidance/include/avoidance/common.h

@@ -221,6 +221,7 @@ void wrapPolar(PolarPoint& p_pol);
 **/
 float nextYaw(const Eigen::Vector3f& u, const Eigen::Vector3f& v);
 
+// todo: is this used?
 void createPoseMsg(Eigen::Vector3f& out_waypt, Eigen::Quaternionf& out_q, const Eigen::Vector3f& in_waypt, float yaw);
 
 /**
@@ -247,6 +248,15 @@ float WARN_UNUSED wrapAngleToPlusMinusPI(float angle);
 * @returns   wrapped angle [deg]
 **/
 float WARN_UNUSED wrapAngleToPlusMinus180(float angle);
+
+/**
+* @brief     returns the correctly wrapped angle difference in degrees
+* @param[in] the first angle in degrees
+* @param[in] the second angle in degrees
+* @returns   the angle between the two given angles [0, 180]
+**/
+float angleDifference(float a, float b);
+
 /**
 * @brief     computes an angular velocity to reach the desired_yaw
 * @param[in] adesired_yaw  [rad]

avoidance/src/common.cpp

@@ -226,6 +226,11 @@ float wrapAngleToPlusMinusPI(float angle) { return angle - 2.0f * M_PI_F * std::
 
 float wrapAngleToPlusMinus180(float angle) { return angle - 360.f * std::floor(angle / 360.f + 0.5f); }
 
+float angleDifference(float a, float b) {
+  float angle = fmod(a - b, 360.f);
+  return angle >= 0.f ? (angle < 180.f) ? angle : angle - 360.f : (angle >= -180.f) ? angle : angle + 360.f;
+}
+
 double getAngularVelocity(float desired_yaw, float curr_yaw) {
   desired_yaw = wrapAngleToPlusMinusPI(desired_yaw);
   float yaw_vel1 = desired_yaw - curr_yaw;

local_planner/cfg/local_planner_node.cfg

@@ -7,12 +7,13 @@ gen = ParameterGenerator()
 
 # local_planner
 
-gen.add("box_radius_",    double_t,    0, "Data points farther away will be discarded", 12.0,  0, 20)
-gen.add("goal_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 10.0, 0, 20.0)
-gen.add("heading_cost_param_", double_t, 0, "Cost function weight for constant heading", 0.5, 0, 20.0)
-gen.add("smooth_cost_param_", double_t, 0, "Cost function weight for path smoothness", 1.5, 0.5, 20.0)
+gen.add("box_radius_",    double_t,    0, "Data points farther away will be discarded", 15.0,  0, 20)
+gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 150.0, 0, 500.0)
+gen.add("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 6.0, 0, 20.0)
+gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 35000.0, 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("tree_heuristic_weight_", double_t, 0, "Weight for the tree heuristic cost", 35.0, 0.0, 50.0)
 gen.add("goal_z_param", double_t, 0, "Height of the goal position", 3.5, -20.0, 20.0)
-gen.add("no_progress_slope_", double_t, 0, "If progress derivative higher than this value the drone rises", -0.0007, -1.0, 1.0)
 gen.add("min_realsense_dist_", double_t, 0, "Discard points closer than that", 0.2, 0, 10)
 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)
@@ -24,12 +25,10 @@ gen.add("smoothing_speed_z_", double_t, 0, "response speed of the smoothing syst
 gen.add("smoothing_margin_degrees_", double_t, 0, "smoothing radius for obstacle cost in cost histogram", 40, 0, 90)
 
 gen.add("use_vel_setpoints_", bool_t, 0, "Enable velocity setpoints (if false, position setpoints are used)", False)
-gen.add("adapt_cost_params_", bool_t, 0, "If no progress towards goal is made, allow rising", True)
 
 # star_planner
-gen.add("children_per_node_",    int_t,    0, "Branching factor of the search tree", 50,  0, 100)
-gen.add("n_expanded_nodes_",    int_t,    0, "Number of nodes expanded in complete tree", 10,  0, 200)
-gen.add("tree_node_distance_",    double_t,    0, "Distance between nodes", 1,  0, 20)
-gen.add("tree_discount_factor_",    double_t,    0, "Discount factor in tree cost function", 0.8,  0, 1)
+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)
 
 exit(gen.generate(PACKAGE, "avoidance", "LocalPlannerNode"))

local_planner/include/local_planner/candidate_direction.h

@@ -1,4 +1,5 @@
 #pragma once
+#include "avoidance/common.h"
 
 namespace avoidance {
 
@@ -12,5 +13,7 @@ struct candidateDirection {
   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); }
 };
 }

local_planner/include/local_planner/cost_parameters.h

@@ -3,10 +3,9 @@
 namespace avoidance {
 
 struct costParameters {
-  float heading_cost_param = 0.5f;
-  float goal_cost_param = 3.f;
-  float smooth_cost_param = 1.5f;
-  float height_change_cost_param = 4.f;
-  float height_change_cost_param_adapted = 4.f;
+  float yaw_cost_param = 0.5f;
+  float pitch_cost_param = 3.f;
+  float velocity_cost_param = 1.5f;
+  float obstacle_cost_param = 5.0f;
 };
 }

local_planner/include/local_planner/local_planner.h

@@ -66,20 +66,12 @@ struct ModelParameters {
 
 class LocalPlanner {
  private:
-  bool adapt_cost_params_;
   bool reach_altitude_ = false;
-  bool waypoint_outside_FOV_ = false;
 
-  size_t dist_incline_window_size_ = 50;
-  int origin_;
-  int tree_age_ = 0;
   int children_per_node_;
   int n_expanded_nodes_;
   int min_num_points_per_cell_ = 3;
 
-  ros::Time integral_time_old_;
-  float no_progress_slope_;
-  float new_yaw_;
   float min_realsense_dist_ = 0.2f;
   float smoothing_margin_degrees_ = 30.f;
   float max_point_age_s_ = 10;
@@ -92,11 +84,7 @@ class LocalPlanner {
   ros::Time last_path_time_;
   ros::Time last_pointcloud_process_time_;
 
-  std::deque<float> goal_dist_incline_;
-  std::vector<float> cost_path_candidates_;
-  std::vector<int> cost_idx_sorted_;
   std::vector<int> closed_set_;
-
   std::vector<TreeNode> tree_;
   std::unique_ptr<StarPlanner> star_planner_;
   costParameters cost_params_;
@@ -106,17 +94,11 @@ class LocalPlanner {
   Eigen::Vector3f position_ = Eigen::Vector3f::Zero();
   Eigen::Vector3f velocity_ = Eigen::Vector3f::Zero();
   Eigen::Vector3f goal_ = Eigen::Vector3f::Zero();
-  Eigen::Vector3f position_old_ = Eigen::Vector3f::Zero();
 
   Histogram polar_histogram_ = Histogram(ALPHA_RES);
   Histogram to_fcu_histogram_ = Histogram(ALPHA_RES);
   Eigen::MatrixXf cost_matrix_;
 
-  /**
-  * @brief     calculates the cost function weights to fly around or over
-  *            obstacles based on the progress towards the goal over time
-  **/
-  void evaluateProgressRate();
   /**
   * @brief     fills message to send histogram to the FCU
   **/
@@ -143,14 +125,12 @@ class LocalPlanner {
   std::vector<uint8_t> cost_image_data_;
   bool use_vel_setpoints_;
   bool currently_armed_ = false;
-  bool smooth_waypoints_ = true;
   bool disable_rise_to_goal_altitude_ = false;
 
   double timeout_startup_;
   double timeout_critical_;
   double timeout_termination_;
   double starting_height_ = 0.0;
-  float speed_ = 1.0f;
   float ground_distance_ = 2.0;
 
   ModelParameters px4_;  // PX4 Firmware paramters
@@ -166,11 +146,12 @@ class LocalPlanner {
   ~LocalPlanner();
 
   /**
-  * @brief     setter method for vehicle position
-  * @param[in] pos, vehicle position message coming from the FCU
+  * @brief     setter method for vehicle position, orientation and velocity
+  * @param[in] pos, vehicle position coming from the FCU
+  * @param[in] vel, vehicle velocity in the FCU frame
   * @param[in] q, vehicle orientation message coming from the FCU
   **/
-  void setPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& q);
+  void setState(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel, const Eigen::Quaternionf& q);
 
   /**
   * @brief     setter method for mission goal
@@ -219,12 +200,6 @@ class LocalPlanner {
   **/
   const pcl::PointCloud<pcl::PointXYZI>& getPointcloud() const;
 
-  /**
-  * @brief     setter method for vehicle velocity
-  * @param[in] vel, velocity message coming from the FCU
-  **/
-  void setCurrentVelocity(const Eigen::Vector3f& vel);
-
   /**
   * @brief     getter method to visualize the tree in rviz
   * @param[in] tree, the whole tree built during planning (vector of nodes)

local_planner/include/local_planner/planner_functions.h

@@ -26,7 +26,7 @@ namespace avoidance {
 * @param[in]  FOV, struct defining current field of view
 * @param[in]  position, current vehicle position
 * @param[in]  min_realsense_dist, minimum sensor range [m]
-* @param[in]  max_age, maximum age (compute cycles) to keep data
+* @param[in]  max_age, maximum age in seconds to keep data
 * @param[in]  elapsed, time elapsed since last processing [s]
 * @param[in]  min_num_points_per_cell, number of points from which on they will
 *             be kept, less points are discarded as noise (careful: 0 is not
@@ -35,7 +35,7 @@ namespace avoidance {
 void processPointcloud(pcl::PointCloud<pcl::PointXYZI>& final_cloud,
                        const std::vector<pcl::PointCloud<pcl::PointXYZ>>& complete_cloud, const Box& histogram_box,
                        const std::vector<FOV>& fov, float yaw_fcu_frame_deg, float pitch_fcu_frame_deg,
-                       const Eigen::Vector3f& position, float min_realsense_dist, int max_age, float elapsed_s,
+                       const Eigen::Vector3f& position, float min_realsense_dist, float max_age, float elapsed_s,
                        int min_num_points_per_cell);
 
 /**
@@ -69,9 +69,8 @@ void compressHistogramElevation(Histogram& new_hist, const Histogram& input_hist
 * @param[out] image of the cost matrix for visualization
 **/
 void getCostMatrix(const Histogram& histogram, const Eigen::Vector3f& goal, const Eigen::Vector3f& position,
-                   float yaw_fcu_frame_deg, const Eigen::Vector3f& last_sent_waypoint,
-                   const costParameters& cost_params, float smoothing_margin_degrees, Eigen::MatrixXf& cost_matrix,
-                   std::vector<uint8_t>& image_data);
+                   const Eigen::Vector3f& velocity, const costParameters& cost_params, float smoothing_margin_degrees,
+                   Eigen::MatrixXf& cost_matrix, std::vector<uint8_t>& image_data);
 
 /**
 * @brief      get the index in the data vector of a color image
@@ -101,20 +100,16 @@ void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int
 
 /**
 * @brief      computes the cost of each direction in the polar histogram
-* @param[in]  e_angle, elevation angle [deg]
-* @param[in]  z_angle, azimuth angle [deg]
+* @param[in]  PolarPoint of the candidate direction
 * @param[in]  goal, current goal position
 * @param[in]  position, current vehicle position
-* @param[in]  position, current vehicle heading in histogram angle convention
-*             [deg]
-* @param[in]  last_sent_waypoint, previous position waypoint
+* @param[in]  velocity, current vehicle velocity
 * @param[in]  cost_params, weights for goal oriented vs smooth behaviour
-* @param[out] distance_cost, cost component due to proximity to obstacles
-* @param[out] other_costs, cost component due to goal and smoothness
+* @returns    a pair with the first value representing the distance cost, and the second the sum of all other costs
 **/
-void costFunction(float e_angle, float z_angle, float obstacle_distance, const Eigen::Vector3f& goal,
-                  const Eigen::Vector3f& position, float yaw_fcu_frame_deg, const Eigen::Vector3f& last_sent_waypoint,
-                  const costParameters& cost_params, float& distance_cost, float& other_costs);
+std::pair<float, float> costFunction(const PolarPoint& candidate_polar, float obstacle_distance,
+                                     const Eigen::Vector3f& goal, const Eigen::Vector3f& position,
+                                     const Eigen::Vector3f& velocity, const costParameters& cost_params);
 
 /**
 * @brief      max-median filtes the cost matrix

local_planner/include/local_planner/star_planner.h

@@ -24,28 +24,18 @@ class StarPlanner {
   int children_per_node_ = 1;
   int n_expanded_nodes_ = 5;
   float tree_node_distance_ = 1.0f;
-  float tree_discount_factor_ = 0.8f;
   float max_path_length_ = 4.f;
-  float curr_yaw_histogram_frame_deg_ = 90.f;
   float smoothing_margin_degrees_ = 30.f;
-
-  std::vector<int> path_node_origins_;
+  float tree_heuristic_weight_ = 10.0f;
 
   pcl::PointCloud<pcl::PointXYZI> cloud_;
 
   Eigen::Vector3f goal_ = Eigen::Vector3f(NAN, NAN, NAN);
-  Eigen::Vector3f projected_last_wp_ = Eigen::Vector3f::Zero();
   Eigen::Vector3f position_ = Eigen::Vector3f(NAN, NAN, NAN);
+  Eigen::Vector3f velocity_ = Eigen::Vector3f(NAN, NAN, NAN);
   costParameters cost_params_;
 
  protected:
-  /**
-  * @brief     computes the cost of a node
-  * @param[in] node_number, sequential number of entry in the tree
-  * @returns
-  **/
-  float treeCostFunction(int node_number) const;
-
   /**
   * @brief     computes the heuristic for a node
   * @param[in] node_number, sequential number of entry in the tree
@@ -56,7 +46,6 @@ class StarPlanner {
  public:
   std::vector<Eigen::Vector3f> path_node_positions_;
   std::vector<int> closed_set_;
-  int tree_age_;
   std::vector<TreeNode> tree_;
 
   StarPlanner();
@@ -68,12 +57,6 @@ class StarPlanner {
   **/
   void setParams(costParameters cost_params);
 
-  /**
-  * @brief     setter method for last sent waypoint
-  * @param[in] projected_last_wp, last waypoint projected out to goal distance
-  **/
-  void setLastDirection(const Eigen::Vector3f& projected_last_wp);
-
   /**
   * @brief     setter method for star_planner pointcloud
   * @param[in] cloud, processed data already cropped and combined with history
@@ -83,9 +66,8 @@ class StarPlanner {
   /**
   * @brief     setter method for vehicle position
   * @param[in] vehicle current position
-  * @param[in] current yaw of the vehicle in FCU frame convention
   **/
-  void setPose(const Eigen::Vector3f& pos, float curr_yaw_fcu_frame_deg);
+  void setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel);
 
   /**
   * @brief     setter method for current goal

local_planner/include/local_planner/tree_node.h

@@ -8,19 +8,16 @@ namespace avoidance {
 
 class TreeNode {
   Eigen::Vector3f position_;
+  Eigen::Vector3f velocity_;
 
  public:
   float total_cost_;
   float heuristic_;
-  float last_e_;
-  float last_z_;
   int origin_;
-  int depth_;
-  float yaw_;
   bool closed_;
 
   TreeNode();
-  TreeNode(int from, int d, const Eigen::Vector3f& pos);
+  TreeNode(int from, const Eigen::Vector3f& pos, const Eigen::Vector3f& vel);
   ~TreeNode() = default;
 
   /**
@@ -35,6 +32,7 @@ class TreeNode {
   * @returns   node position in 3D cartesian coordinates
   **/
   Eigen::Vector3f getPosition() const;
+  Eigen::Vector3f getVelocity() const;
 };
 }