Remove ground cropping

local_planner/CMakeLists.txt

@@ -138,7 +138,6 @@ include_directories(
 set(LOCAL_PLANNER_CPP_FILES   "src/nodes/local_planner.cpp"
                               "src/nodes/waypoint_generator.cpp"
                               "src/nodes/tree_node.cpp"
-                              "src/nodes/box.cpp"
                               "src/nodes/star_planner.cpp"
                               "src/nodes/planner_functions.cpp"
                               "src/nodes/local_planner_node.cpp"

local_planner/cfg/local_planner_node.cfg

@@ -7,10 +7,10 @@ gen = ParameterGenerator()
 
 # local_planner
 
-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("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)
+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("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)

local_planner/include/local_planner/local_planner.h

@@ -4,7 +4,6 @@
 #include <sensor_msgs/image_encodings.h>
 #include "avoidance/histogram.h"
 #include "avoidance_output.h"
-#include "box.h"
 #include "candidate_direction.h"
 #include "cost_parameters.h"
 #include "planner_functions.h"
@@ -77,6 +76,7 @@ class LocalPlanner {
   float max_point_age_s_ = 10;
   float yaw_fcu_frame_deg_ = 0.0f;
   float pitch_fcu_frame_deg_ = 0.0f;
+  float sensor_range_ = 12.0f;
 
   std::vector<FOV> fov_fcu_frame_;
 
@@ -120,7 +120,6 @@ class LocalPlanner {
   void generateHistogramImage(Histogram& histogram);
 
  public:
-  Box histogram_box_;
   std::vector<uint8_t> histogram_image_data_;
   std::vector<uint8_t> cost_image_data_;
   bool use_vel_setpoints_;
@@ -131,7 +130,6 @@ class LocalPlanner {
   double timeout_critical_;
   double timeout_termination_;
   double starting_height_ = 0.0;
-  float ground_distance_ = 2.0;
 
   ModelParameters px4_;  // PX4 Firmware paramters
 
@@ -172,6 +170,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_; }
 
   /**
   * @brief     getter method for current goal

local_planner/include/local_planner/local_planner_visualization.h

@@ -48,15 +48,6 @@ class LocalPlannerVisualization {
   **/
   void publishGoal(const geometry_msgs::Point& goal) const;
 
-  /**
-  * @brief       Visualization of the bounding box used to crop the pointcloud
-  * @params[in]  drone_pos, current position of the drone
-  * @params[in]  box_radius, the radius of the bounding box
-  * @params[in]  plane_height, the height above ground at which the pointcloud
-  *              is additionally cropped
-  **/
-  void publishBox(const Eigen::Vector3f& drone_pos, float box_radius, float plane_height) const;
-
   /**
   * @brief       Visualization of the data used during takeoff
   * @params[in]  take_off_pose, pose at which the vehicle was armed
@@ -118,14 +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 ground
-  * @params[in]  drone_pos, location of the drone at the current timestep
-  * @params[in]  box_radius, the radius of the bounding box
-  * @params[in]  ground_distance, measured distance to ground
-  **/
-  void publishGround(const Eigen::Vector3f& drone_pos, float box_radius, float ground_distance) const;
-
   /**
   * @brief       Visualization of the offtrack state
   * @params[in]  closest_pt, vehicle position projection on the line previous to

local_planner/include/local_planner/planner_functions.h

@@ -3,7 +3,6 @@
 
 #include "avoidance/common.h"
 #include "avoidance/histogram.h"
-#include "box.h"
 #include "candidate_direction.h"
 #include "cost_parameters.h"
 
@@ -22,7 +21,6 @@ namespace avoidance {
 *             the data from the last timestep
 * @param      final_cloud, processed data to be used for planning
 * @param[in]  complete_cloud, array of pointclouds from the sensors
-* @param[in]  histogram_box, geometry definition of the bounding box
 * @param[in]  FOV, struct defining current field of view
 * @param[in]  position, current vehicle position
 * @param[in]  min_realsense_dist, minimum sensor range [m]
@@ -33,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 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, float 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, float max_age, float elapsed_s, int min_num_points_per_cell);
 
 /**
 * @brief      calculates a histogram from the current frame pointcloud around

local_planner/include/local_planner/star_planner.h

@@ -2,7 +2,6 @@
 #define STAR_PLANNER_H
 
 #include "avoidance/histogram.h"
-#include "box.h"
 #include "cost_parameters.h"
 
 #include <Eigen/Dense>

local_planner/src/nodes/local_planner.cpp

@@ -28,7 +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_);
+  //  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_;
@@ -78,12 +79,10 @@ void LocalPlanner::runPlanner() {
   ROS_INFO("\033[1;35m[OA] Planning started, using %i cameras\n \033[0m",
            static_cast<int>(original_cloud_vector_.size()));
 
-  histogram_box_.setBoxLimits(position_, ground_distance_);
-
   float elapsed_since_last_processing = static_cast<float>((ros::Time::now() - last_pointcloud_process_time_).toSec());
-  processPointcloud(final_cloud_, original_cloud_vector_, histogram_box_, 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();
@@ -114,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 / histogram_box_.radius_ : 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)));
     }
   }
@@ -167,7 +166,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 = histogram_box_.radius_;
+  msg.range_max = sensor_range_;
   msg.ranges.reserve(GRID_LENGTH_Z);
 
   for (int i = 0; i < GRID_LENGTH_Z; ++i) {
@@ -176,7 +175,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 : histogram_box_.radius_ + 1.0f);
+    msg.ranges.push_back(dist > min_realsense_dist_ ? dist : sensor_range_ + 1.0f);
   }
 
   distance_data_ = msg;
@@ -188,7 +187,7 @@ void LocalPlanner::updateObstacleDistanceMsg() {
   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 = histogram_box_.radius_;
+  msg.range_max = sensor_range_;
 
   distance_data_ = msg;
 }
@@ -236,7 +235,7 @@ avoidanceOutput LocalPlanner::getAvoidanceOutput() const {
   float accel_ramp_time = px4_.param_mpc_acc_hor / px4_.param_mpc_jerk_max;
   float a = 1;
   float b = 2 * px4_.param_mpc_acc_hor * accel_ramp_time;
-  float c = 2 * -px4_.param_mpc_acc_hor * histogram_box_.radius_;
+  float c = 2 * -px4_.param_mpc_acc_hor * sensor_range_;
   float limited_speed = (-b + std::sqrt(b * b - 4 * a * c)) / (2 * a);
 
   float max_speed = std::min(px4_.param_mpc_xy_cruise, limited_speed);

local_planner/src/nodes/local_planner_node.cpp

@@ -185,14 +185,6 @@ void LocalPlannerNode::updatePlannerInfo() {
     new_goal_ = false;
   }
 
-  // update ground distance
-  if (ros::Time::now() - ground_distance_msg_.header.stamp < ros::Duration(0.5)) {
-    local_planner_->ground_distance_ = ground_distance_msg_.bottom_clearance;
-  } else {
-    local_planner_->ground_distance_ = 2.0;  // in case where no range data is
-    // available assume vehicle is close to ground
-  }
-
   // update last sent waypoint
   local_planner_->last_sent_waypoint_ = toEigen(newest_waypoint_position_);
 }
@@ -359,8 +351,6 @@ void LocalPlannerNode::fcuInputGoalCallback(const mavros_msgs::Trajectory& msg)
 void LocalPlannerNode::distanceSensorCallback(const mavros_msgs::Altitude& msg) {
   if (!std::isnan(msg.bottom_clearance)) {
     ground_distance_msg_ = msg;
-    visualizer_.publishGround(local_planner_->getPosition(), local_planner_->histogram_box_.radius_,
-                              local_planner_->ground_distance_);
   }
 }