Plan setpoints and positions separately

This commit changes the star planner to distinguish between setpoints sent to the position controller and the actual positions we would then end up in.
PX4 · Jul 30, 2019 · 16c0b25 · 16c0b25
1 parent a294180
commit 16c0b25
Show file tree

Hide file tree

Showing 19 changed files with 136 additions and 131 deletions.
diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg
@@ -6,7 +6,6 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
 
 # local_planner
-
 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", 25.0, 0, 30.0)
 gen.add("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 3.0, 0, 20.0)
@@ -23,12 +22,11 @@ gen.add("min_num_points_per_cell_", int_t, 0, "minimum number of points in one a
 gen.add("smoothing_speed_xy_", double_t, 0, "response speed of the smoothing system in xy (set to 0 to disable)", 10, 0, 30)
 gen.add("smoothing_speed_z_", double_t, 0, "response speed of the smoothing system in z (set to 0 to disable)", 3, 0, 30)
 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)
 
 # 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 in seconds between nodes", 0.5,  0.01, 2)
+gen.add("tree_node_duration_",    double_t,    0, "Distance in seconds between nodes", 0.5,  0.01, 2)
 
 exit(gen.generate(PACKAGE, "avoidance", "LocalPlannerNode"))
diff --git a/local_planner/cfg/vehicle.yaml b/local_planner/cfg/vehicle.yaml
@@ -1,56 +1,55 @@
-# 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_realsense_dist_: 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: ''
       use_vel_setpoints_: false
+      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_realsense_dist_: 0.2
-  n_expanded_nodes_: 10
-  no_progress_slope_: -0.0007
-  smooth_cost_param_: 1.5
+  n_expanded_nodes_: 40
+  obstacle_cost_param_: 8.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
   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
   use_vel_setpoints_: false
+  velocity_cost_param_: 6000.0
+  yaw_cost_param_: 3.0
 state: []
diff --git a/local_planner/include/local_planner/avoidance_output.h b/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
 };
 }
diff --git a/local_planner/include/local_planner/local_planner.h b/local_planner/include/local_planner/local_planner.h
@@ -78,6 +78,7 @@ class LocalPlanner {
   float yaw_fcu_frame_deg_ = 0.0f;
   float pitch_fcu_frame_deg_ = 0.0f;
   float sensor_range_ = 12.0f;
+  float tree_node_duration_ = 0.5f;
 
   std::vector<FOV> fov_fcu_frame_;
 
@@ -203,11 +204,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

diff --git a/local_planner/include/local_planner/local_planner_visualization.h b/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

diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h
@@ -142,11 +142,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
diff --git a/local_planner/include/local_planner/star_planner.h b/local_planner/include/local_planner/star_planner.h
@@ -21,12 +21,12 @@ namespace avoidance {
 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.0f;
 
   pcl::PointCloud<pcl::PointXYZI> cloud_;
 
@@ -45,7 +45,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_;
 

diff --git a/local_planner/include/local_planner/trajectory_simulator.h b/local_planner/include/local_planner/trajectory_simulator.h
@@ -6,13 +6,23 @@
 namespace avoidance {
 
 struct simulation_state {
+  simulation_state(float t, const Eigen::Vector3f& p, const Eigen::Vector3f& v, const Eigen::Vector3f& a)
+      : time(t), position(p), velocity(v), acceleration(a){};
+  simulation_state(){};
   float time = NAN;
   Eigen::Vector3f position = NAN * Eigen::Vector3f::Ones();
   Eigen::Vector3f velocity = NAN * Eigen::Vector3f::Ones();
   Eigen::Vector3f acceleration = NAN * Eigen::Vector3f::Ones();
 };
 
 struct simulation_limits {
+  simulation_limits(float max_z, float min_z, float max_xy_n, float max_acc_n, float max_jerk_n)
+      : max_z_velocity(max_z),
+        min_z_velocity(min_z),
+        max_xy_velocity_norm(max_xy_n),
+        max_acceleration_norm(max_acc_n),
+        max_jerk_norm(max_jerk_n){};
+  simulation_limits(){};
   float max_z_velocity = NAN;
   float min_z_velocity = NAN;
   float max_xy_velocity_norm = NAN;

diff --git a/local_planner/include/local_planner/tree_node.h b/local_planner/include/local_planner/tree_node.h
@@ -13,10 +13,11 @@ class TreeNode {
   float heuristic_;
   int origin_;
   bool closed_;
-  simulation_state state;
+  simulation_state state;    // State containing position, velocity and time of the drone
+  Eigen::Vector3f setpoint;  // Setpoint required to send to PX4 in order to get to this state
 
-  TreeNode();
-  TreeNode(int from, const simulation_state& start_state);
+  TreeNode() = delete;
+  TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp);
   ~TreeNode() = default;
 
   /**
@@ -27,11 +28,15 @@ class TreeNode {
   void setCosts(float h, float c);
 
   /**
-  * @brief     getter method for tree node position
+  * @defgroup  getterFunctions
+  * @brief     getter methods for tree node position, velocity and setpoint
   * @returns   node position in 3D cartesian coordinates
+  * @{
   **/
   Eigen::Vector3f getPosition() const;
   Eigen::Vector3f getVelocity() const;
+  Eigen::Vector3f getSetpoint() const;
+  /** @} */  // end of doxygen group getterFunctions
 };
 }
 

diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp
@@ -43,6 +43,7 @@ void LocalPlanner::dynamicReconfigureSetParams(avoidance::LocalPlannerNodeConfig
   children_per_node_ = config.children_per_node_;
   n_expanded_nodes_ = config.n_expanded_nodes_;
   smoothing_margin_degrees_ = static_cast<float>(config.smoothing_margin_degrees_);
+  tree_node_duration_ = static_cast<float>(config.tree_node_duration_);
 
   if (getGoal().z() != config.goal_z_param) {
     auto goal = getGoal();
@@ -219,10 +220,10 @@ void LocalPlanner::setDefaultPx4Parameters() {
 }
 
 void LocalPlanner::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 {
   tree = star_planner_->tree_;
   closed_set = star_planner_->closed_set_;
-  path_node_positions = star_planner_->path_node_positions_;
+  path_node_setpoints = star_planner_->path_node_setpoints_;
 }
 
 void LocalPlanner::getObstacleDistanceData(sensor_msgs::LaserScan& obstacle_distance) {
@@ -249,10 +250,10 @@ avoidanceOutput LocalPlanner::getAvoidanceOutput() const {
 
   out.cruise_velocity = max_speed;
   out.last_path_time = last_path_time_;
-
+  out.tree_node_duration = tree_node_duration_;
   out.take_off_pose = take_off_pose_;
 
-  out.path_node_positions = star_planner_->path_node_positions_;
+  out.path_node_setpoints = star_planner_->path_node_setpoints_;
   return out;
 }
 }
diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp
@@ -45,9 +45,9 @@ void LocalPlannerVisualization::visualizePlannerData(const LocalPlanner& planner
   // visualize tree calculation
   std::vector<TreeNode> tree;
   std::vector<int> closed_set;
-  std::vector<Eigen::Vector3f> path_node_positions;
-  planner.getTree(tree, closed_set, path_node_positions);
-  publishTree(tree, closed_set, path_node_positions);
+  std::vector<Eigen::Vector3f> path_node_setpoints;
+  planner.getTree(tree, closed_set, path_node_setpoints);
+  publishTree(tree, closed_set, path_node_setpoints);
 
   // visualize goal
   publishGoal(toPoint(planner.getGoal()));
@@ -138,7 +138,7 @@ void LocalPlannerVisualization::publishOfftrackPoints(Eigen::Vector3f& closest_p
 }
 
 void LocalPlannerVisualization::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 {
   visualization_msgs::Marker tree_marker;
   tree_marker.header.frame_id = "local_origin";
   tree_marker.header.stamp = ros::Time::now();
@@ -175,10 +175,10 @@ void LocalPlannerVisualization::publishTree(const std::vector<TreeNode>& tree, c
     tree_marker.points.push_back(p2);
   }
 
-  path_marker.points.reserve(path_node_positions.size() * 2);
-  for (size_t i = 1; i < path_node_positions.size(); i++) {
-    path_marker.points.push_back(toPoint(path_node_positions[i - 1]));
-    path_marker.points.push_back(toPoint(path_node_positions[i]));
+  path_marker.points.reserve(path_node_setpoints.size() * 2);
+  for (size_t i = 1; i < path_node_setpoints.size(); i++) {
+    path_marker.points.push_back(toPoint(path_node_setpoints[i - 1]));
+    path_marker.points.push_back(toPoint(path_node_setpoints[i]));
   }
 
   complete_tree_pub_.publish(tree_marker);

diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp
@@ -313,33 +313,36 @@ std::pair<float, float> costFunction(const PolarPoint& candidate_polar, float ob
   return std::pair<float, float>(distance_cost, velocity_cost + yaw_cost + pitch_cost);
 }
 
-bool getSetpointFromPath(const std::vector<Eigen::Vector3f>& path, const ros::Time& path_generation_time,
-                         float velocity, Eigen::Vector3f& setpoint) {
-  int i = path.size();
+bool interpolateBetweenSetpoints(const std::vector<Eigen::Vector3f>& setpoint_array,
+                                 const ros::Time& path_generation_time, float tree_node_duration,
+                                 Eigen::Vector3f& setpoint) {
+  int i = setpoint_array.size();
   // path contains nothing meaningful
   if (i < 2) {
+    ROS_WARN("Path contains fewer than two nodes, this is not a path!");
     return false;
   }
 
   // path only has one segment: return end of that segment as setpoint
   if (i == 2) {
-    setpoint = path[0];
+    ROS_INFO("Path contains only two nodes, using second node as setpoint!");
+    setpoint = setpoint_array[0];
     return true;
   }
 
   // step through the path until the point where we should be if we had traveled perfectly with velocity along it
-  Eigen::Vector3f path_segment = path[i - 3] - path[i - 2];
-  float distance_left = (ros::Time::now() - path_generation_time).toSec() * velocity;
-  setpoint = path[i - 2] + (distance_left / path_segment.norm()) * path_segment;
-
-  for (i = path.size() - 3; i > 0 && distance_left > path_segment.norm(); --i) {
-    distance_left -= path_segment.norm();
-    path_segment = path[i - 1] - path[i];
-    setpoint = path[i] + (distance_left / path_segment.norm()) * path_segment;
+  const int seg = i - 2 - std::floor((ros::Time::now() - path_generation_time).toSec() / tree_node_duration);
+
+  // path expired
+  if (seg < 1) {
+    ROS_WARN("Path has expired!");
+    return false;
   }
 
-  // If we excited because we're past the last node of the path, the path is no longer valid!
-  return distance_left < path_segment.norm();
+  Eigen::Vector3f path_segment = setpoint_array[seg - 1] - setpoint_array[seg];
+  float distance_left = fmod((ros::Time::now() - path_generation_time).toSec(), tree_node_duration);
+  setpoint = setpoint_array[seg] + (distance_left / tree_node_duration) * path_segment;
+  return true;
 }
 
 void printHistogram(Histogram& histogram) {