Plan dynamically feasible trajectories

PX4 · Jul 23, 2019 · b4551ab · b4551ab
1 parent cec8743
commit b4551ab
Show file tree

Hide file tree

Showing 12 changed files with 102 additions and 77 deletions.
diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg
@@ -10,8 +10,8 @@ gen = ParameterGenerator()
 gen.add("sensor_range_",    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", 20.0, 0, 30.0)
 gen.add("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 2.0, 0, 20.0)
-gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 1200, 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", 500, 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", 4.5, 0.0, 30.0)
 gen.add("goal_z_param", double_t, 0, "Height of the goal position", 3.5, -20.0, 20.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)
@@ -28,6 +28,6 @@ gen.add("use_vel_setpoints_", bool_t, 0, "Enable velocity setpoints (if false, p
 # 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_distance_",    double_t,    0, "Distance in seconds between nodes", 0.5,  0.01, 2)
 
 exit(gen.generate(PACKAGE, "avoidance", "LocalPlannerNode"))
diff --git a/local_planner/include/local_planner/candidate_direction.h b/local_planner/include/local_planner/candidate_direction.h
@@ -1,4 +1,7 @@
 #pragma once
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <cmath>
 #include "avoidance/common.h"
 
 namespace avoidance {
@@ -15,5 +18,10 @@ struct candidateDirection {
   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));
+  }
 };
 }
diff --git a/local_planner/include/local_planner/local_planner.h b/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>
@@ -120,7 +121,7 @@ class LocalPlanner {
   void generateHistogramImage(Histogram& histogram);
 
  public:
-  //Box histogram_box_;
+  // Box histogram_box_;
   std::vector<uint8_t> histogram_image_data_;
   std::vector<uint8_t> cost_image_data_;
   bool use_vel_setpoints_;
@@ -131,8 +132,7 @@ class LocalPlanner {
   double timeout_critical_;
   double timeout_termination_;
   double starting_height_ = 0.0;
-  //float ground_distance_ = 2.0;
-
+  // float ground_distance_ = 2.0;
 
   ModelParameters px4_;  // PX4 Firmware paramters
 
@@ -173,7 +173,7 @@ class LocalPlanner {
   float getHFOV(int i) { return i < fov_fcu_frame_.size() ? fov_fcu_frame_[i].h_fov_deg : 0.f; }
   float getVFOV(int i) { return i < fov_fcu_frame_.size() ? fov_fcu_frame_[i].v_fov_deg : 0.f; }
   const std::vector<FOV>& getFOV() const { return fov_fcu_frame_; }
-  float getSensorRange() const {return sensor_range_;}
+  float getSensorRange() const { return sensor_range_; }
 
   /**
   * @brief     getter method for current goal

diff --git a/local_planner/include/local_planner/local_planner_visualization.h b/local_planner/include/local_planner/local_planner_visualization.h
@@ -48,8 +48,6 @@ class LocalPlannerVisualization {
   **/
   void publishGoal(const geometry_msgs::Point& goal) const;
 
-
-
   /**
   * @brief       Visualization of the data used during takeoff
   * @params[in]  take_off_pose, pose at which the vehicle was armed
@@ -111,8 +109,6 @@ class LocalPlannerVisualization {
   void publishCurrentSetpoint(const geometry_msgs::Twist& wp, const waypoint_choice& waypoint_type,
                               const geometry_msgs::Point& newest_pos) const;
 
-
-
   /**
   * @brief       Visualization of the offtrack state
   * @params[in]  closest_pt, vehicle position projection on the line previous to

diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h
@@ -31,10 +31,9 @@ namespace avoidance {
 *             a valid input here)
 **/
 void processPointcloud(pcl::PointCloud<pcl::PointXYZI>& final_cloud,
-                       const std::vector<pcl::PointCloud<pcl::PointXYZ>>& complete_cloud,
-                       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,
-                       int min_num_points_per_cell);
+                       const std::vector<pcl::PointCloud<pcl::PointXYZ>>& complete_cloud, 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, int min_num_points_per_cell);
 
 /**
 * @brief      calculates a histogram from the current frame pointcloud around

diff --git a/local_planner/include/local_planner/star_planner.h b/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>
 
@@ -32,6 +33,7 @@ class StarPlanner {
   Eigen::Vector3f position_ = Eigen::Vector3f(NAN, NAN, NAN);
   Eigen::Vector3f velocity_ = Eigen::Vector3f(NAN, NAN, NAN);
   costParameters cost_params_;
+  simulation_limits lims_;
 
  protected:
   /**
@@ -52,8 +54,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);
 
   /**
   * @brief     setter method for star_planner pointcloud

diff --git a/local_planner/include/local_planner/tree_node.h b/local_planner/include/local_planner/tree_node.h
@@ -3,21 +3,20 @@
 
 #include <Eigen/Core>
 #include <vector>
+#include "trajectory_simulator.h"
 
 namespace avoidance {
 
 class TreeNode {
-  Eigen::Vector3f position_;
-  Eigen::Vector3f velocity_;
-
  public:
   float total_cost_;
   float heuristic_;
   int origin_;
   bool closed_;
+  simulation_state state;
 
   TreeNode();
-  TreeNode(int from, const Eigen::Vector3f& pos, const Eigen::Vector3f& vel);
+  TreeNode(int from, const simulation_state& start_state);
   ~TreeNode() = default;
 
   /**

diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp
@@ -28,8 +28,8 @@ void LocalPlanner::setState(const Eigen::Vector3f& pos, const Eigen::Vector3f& v
 
 // set parameters changed by dynamic rconfigure
 void LocalPlanner::dynamicReconfigureSetParams(avoidance::LocalPlannerNodeConfig& config, uint32_t level) {
-//  histogram_box_.radius_ = static_cast<float>(config.box_radius_);
-sensor_range_ = static_cast<float>(config.sensor_range_);
+  //  histogram_box_.radius_ = static_cast<float>(config.box_radius_);
+  sensor_range_ = static_cast<float>(config.sensor_range_);
   cost_params_.pitch_cost_param = config.pitch_cost_param_;
   cost_params_.yaw_cost_param = config.yaw_cost_param_;
   cost_params_.velocity_cost_param = config.velocity_cost_param_;
@@ -80,9 +80,9 @@ void LocalPlanner::runPlanner() {
            static_cast<int>(original_cloud_vector_.size()));
 
   float elapsed_since_last_processing = static_cast<float>((ros::Time::now() - last_pointcloud_process_time_).toSec());
-  processPointcloud(final_cloud_, original_cloud_vector_, fov_fcu_frame_, yaw_fcu_frame_deg_,
-                    pitch_fcu_frame_deg_, position_, min_realsense_dist_, max_point_age_s_,
-                    elapsed_since_last_processing, min_num_points_per_cell_);
+  processPointcloud(final_cloud_, original_cloud_vector_, fov_fcu_frame_, yaw_fcu_frame_deg_, pitch_fcu_frame_deg_,
+                    position_, min_realsense_dist_, max_point_age_s_, elapsed_since_last_processing,
+                    min_num_points_per_cell_);
   last_pointcloud_process_time_ = ros::Time::now();
 
   determineStrategy();
@@ -113,7 +113,7 @@ void LocalPlanner::generateHistogramImage(Histogram& histogram) {
   for (int e = GRID_LENGTH_E - 1; e >= 0; e--) {
     for (int z = 0; z < GRID_LENGTH_Z; z++) {
       float dist = histogram.get_dist(e, z);
-      float depth_val = dist > 0.01f ? 255.f - 255.f * dist / sensor_range_: 0.f;
+      float depth_val = dist > 0.01f ? 255.f - 255.f * dist / sensor_range_ : 0.f;
       histogram_image_data_.push_back((int)std::max(0.0f, std::min(255.f, depth_val)));
     }
   }
@@ -150,7 +150,14 @@ void LocalPlanner::determineStrategy() {
       getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_,
                     cost_matrix_, cost_image_data_);
 
-      star_planner_->setParams(cost_params_);
+      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_vel_max_dn;
+      lims.max_xy_velocity_norm = px4_.param_mpc_xy_cruise;
+      lims.max_acceleration_norm = px4_.param_mpc_acc_hor;
+      lims.max_jerk_norm = px4_.param_mpc_jerk_max;
+      star_planner_->setParams(cost_params_, lims);
       star_planner_->setPointcloud(final_cloud_);
 
       // build search tree
@@ -166,7 +173,7 @@ void LocalPlanner::updateObstacleDistanceMsg(Histogram hist) {
   msg.header.frame_id = "local_origin";
   msg.angle_increment = static_cast<double>(ALPHA_RES) * M_PI / 180.0;
   msg.range_min = min_realsense_dist_;
-  msg.range_max = sensor_range_;//histogram_box_.radius_;
+  msg.range_max = sensor_range_;  // histogram_box_.radius_;
   msg.ranges.reserve(GRID_LENGTH_Z);
 
   for (int i = 0; i < GRID_LENGTH_Z; ++i) {
@@ -175,7 +182,7 @@ void LocalPlanner::updateObstacleDistanceMsg(Histogram hist) {
     float dist = hist.get_dist(0, j);
 
     // special case: distance of 0 denotes 'no obstacle in sight'
-    msg.ranges.push_back(dist > min_realsense_dist_ ? dist : sensor_range_+ 1.0f);
+    msg.ranges.push_back(dist > min_realsense_dist_ ? dist : sensor_range_ + 1.0f);
   }
 
   distance_data_ = msg;

diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp
@@ -207,8 +207,6 @@ void LocalPlannerVisualization::publishGoal(const geometry_msgs::Point& goal) co
   marker_goal_pub_.publish(marker_goal);
 }
 
-
-
 void LocalPlannerVisualization::publishReachHeight(const Eigen::Vector3f& take_off_pose, float starting_height) const {
   visualization_msgs::Marker m;
   m.header.frame_id = "local_origin";

diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp
@@ -10,10 +10,9 @@ namespace avoidance {
 
 // trim the point cloud so that only one valid point per histogram cell is around
 void processPointcloud(pcl::PointCloud<pcl::PointXYZI>& final_cloud,
-                       const std::vector<pcl::PointCloud<pcl::PointXYZ>>& complete_cloud,
-                       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,
-                       int min_num_points_per_cell) {
+                       const std::vector<pcl::PointCloud<pcl::PointXYZ>>& complete_cloud, 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, int min_num_points_per_cell) {
   pcl::PointCloud<pcl::PointXYZI> old_cloud;
   std::swap(final_cloud, old_cloud);
   final_cloud.points.clear();
@@ -30,16 +29,16 @@ void processPointcloud(pcl::PointCloud<pcl::PointXYZI>& final_cloud,
     for (const pcl::PointXYZ& xyz : cloud) {
       // Check if the point is invalid
       if (!std::isnan(xyz.x) && !std::isnan(xyz.y) && !std::isnan(xyz.z)) {
-          distance = (position - toEigen(xyz)).norm();
-          if (distance > min_realsense_dist ) {
-            // subsampling the cloud
-            PolarPoint p_pol = cartesianToPolarHistogram(toEigen(xyz), position);
-            Eigen::Vector2i p_ind = polarToHistogramIndex(p_pol, ALPHA_RES / 2);
-            histogram_points_counter(p_ind.y(), p_ind.x())++;
-            if (histogram_points_counter(p_ind.y(), p_ind.x()) == min_num_points_per_cell) {
-              final_cloud.points.push_back(toXYZI(toEigen(xyz), 0));
-            }
+        distance = (position - toEigen(xyz)).norm();
+        if (distance > min_realsense_dist) {
+          // subsampling the cloud
+          PolarPoint p_pol = cartesianToPolarHistogram(toEigen(xyz), position);
+          Eigen::Vector2i p_ind = polarToHistogramIndex(p_pol, ALPHA_RES / 2);
+          histogram_points_counter(p_ind.y(), p_ind.x())++;
+          if (histogram_points_counter(p_ind.y(), p_ind.x()) == min_num_points_per_cell) {
+            final_cloud.points.push_back(toXYZI(toEigen(xyz), 0));
           }
+        }
       }
     }
   }
@@ -48,25 +47,24 @@ void processPointcloud(pcl::PointCloud<pcl::PointXYZI>& final_cloud,
   for (const pcl::PointXYZI& xyzi : old_cloud) {
     // adding older points if not expired and space is free according to new
     // cloud
-      distance = (position - toEigen(xyzi)).norm();
-
-        PolarPoint p_pol = cartesianToPolarHistogram(toEigen(xyzi), position);
-        PolarPoint p_pol_fcu = cartesianToPolarFCU(toEigen(xyzi), position);
-        p_pol_fcu.e -= pitch_fcu_frame_deg;
-        p_pol_fcu.z -= yaw_fcu_frame_deg;
-        wrapPolar(p_pol_fcu);
-        Eigen::Vector2i p_ind = polarToHistogramIndex(p_pol, ALPHA_RES / 2);
-
-        // only remember point if it's in a cell not previously populated by
-        // complete_cloud, as well as outside FOV and 'young' enough
-        if (histogram_points_counter(p_ind.y(), p_ind.x()) < min_num_points_per_cell && xyzi.intensity < max_age &&
-            !pointInsideFOV(fov, p_pol_fcu)) {
-          final_cloud.points.push_back(toXYZI(toEigen(xyzi), xyzi.intensity + elapsed_s));
-
-          // to indicate that this cell now has a point
-          histogram_points_counter(p_ind.y(), p_ind.x()) = min_num_points_per_cell;
-        }
-
+    distance = (position - toEigen(xyzi)).norm();
+
+    PolarPoint p_pol = cartesianToPolarHistogram(toEigen(xyzi), position);
+    PolarPoint p_pol_fcu = cartesianToPolarFCU(toEigen(xyzi), position);
+    p_pol_fcu.e -= pitch_fcu_frame_deg;
+    p_pol_fcu.z -= yaw_fcu_frame_deg;
+    wrapPolar(p_pol_fcu);
+    Eigen::Vector2i p_ind = polarToHistogramIndex(p_pol, ALPHA_RES / 2);
+
+    // only remember point if it's in a cell not previously populated by
+    // complete_cloud, as well as outside FOV and 'young' enough
+    if (histogram_points_counter(p_ind.y(), p_ind.x()) < min_num_points_per_cell && xyzi.intensity < max_age &&
+        !pointInsideFOV(fov, p_pol_fcu)) {
+      final_cloud.points.push_back(toXYZI(toEigen(xyzi), xyzi.intensity + elapsed_s));
+
+      // to indicate that this cell now has a point
+      histogram_points_counter(p_ind.y(), p_ind.x()) = min_num_points_per_cell;
+    }
   }
 
   final_cloud.header.stamp = complete_cloud[0].header.stamp;

diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp
@@ -19,7 +19,10 @@ void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerN
   smoothing_margin_degrees_ = static_cast<float>(config.smoothing_margin_degrees_);
 }
 
-void StarPlanner::setParams(costParameters cost_params) { cost_params_ = cost_params; }
+void StarPlanner::setParams(const costParameters& cost_params, const simulation_limits& limits) {
+  cost_params_ = cost_params;
+  lims_ = limits;
+}
 
 void StarPlanner::setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel) {
   position_ = pos;
@@ -48,7 +51,12 @@ void StarPlanner::buildLookAheadTree() {
   closed_set_.clear();
 
   // insert first node
-  tree_.push_back(TreeNode(0, position_, velocity_));
+  simulation_state start_state;
+  start_state.position = position_;
+  start_state.velocity = velocity_;
+  start_state.acceleration = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
+  start_state.time = ros::Time::now().toSec();
+  tree_.push_back(TreeNode(0, start_state));
   tree_.back().setCosts(treeHeuristicFunction(0), treeHeuristicFunction(0));
 
   int origin = 0;
@@ -76,22 +84,28 @@ void StarPlanner::buildLookAheadTree() {
       // insert new nodes
       int children = 0;
       for (candidateDirection candidate : candidate_vector) {
-        PolarPoint candidate_polar = candidate.toPolar(tree_node_distance_);
-        Eigen::Vector3f node_location = polarHistogramToCartesian(candidate_polar, origin_position);
-        Eigen::Vector3f node_velocity = node_location - origin_position;  // todo: simulate!
+        simulation_state state = tree_[origin].state;
+        TrajectorySimulator sim(lims_, state, 0.05f); // todo: parameterize simulation step size [s]
+        std::vector<simulation_state> trajectory = sim.generate_trajectory(candidate.toEigen(), tree_node_distance_);
+
+        // Ignore node if it brings us farther away from the goal
+        // todo: this breaks being able to get out of concave obstacles! But it helps to not "overplan"
+        if ((trajectory.back().position - goal_).norm() > (trajectory.front().position - goal_).norm()) {
+          continue;
+        }
 
         // check if another close node has been added
         int close_nodes = 0;
         for (size_t i = 0; i < tree_.size(); i++) {
-          float dist = (tree_[i].getPosition() - node_location).norm();
+          float dist = (tree_[i].getPosition() - trajectory.back().position).norm();
           if (dist < 0.2f) {
             close_nodes++;
             break;
           }
         }
 
         if (children < children_per_node_ && close_nodes == 0) {
-          tree_.push_back(TreeNode(origin, node_location, node_velocity));
+          tree_.push_back(TreeNode(origin, trajectory.back()));
           float h = treeHeuristicFunction(tree_.size() - 1);
           tree_.back().heuristic_ = h;
           tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + candidate.cost + h;