KD-tree attempt

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
@@ -353,6 +353,28 @@ pcl::PointCloud<pcl::PointXYZ> removeNaNAndGetMaxima(pcl::PointCloud<pcl::PointX
 **/
 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);
   return ev3;
@@ -373,6 +395,11 @@ inline Eigen::Vector3f toEigen(const pcl::PointXYZI& p) {
   return ev3;
 }
 
+inline std::array<float, 3> toArray(const Eigen::Vector3f& ev3) {
+  std::array<float, 3> ret{ev3.x(), ev3.y(), ev3.z()};
+  return ret;
+}
+
 inline Eigen::Quaternionf toEigen(const geometry_msgs::Quaternion& gmq) {
   Eigen::Quaternionf eqf;
   eqf.x() = gmq.x;