Reformat.

Author: sikang

motion_primitive_library

@@ -15,7 +15,7 @@ namespace MPL {
  * @brief Motion primitive planner using point cloud
  */
 class EllipsoidPlanner : public PlannerBase<3, Waypoint3D> {
-public:
+ public:
   /**
    * @brief Simple constructor
    * @param verbose enable print out
@@ -32,6 +32,6 @@ class EllipsoidPlanner : public PlannerBase<3, Waypoint3D> {
     ENV_.reset(new MPL::env_cloud(obs, r, ori, dim));
   }
 };
-}
+}  // namespace MPL
 
 #endif

mpl_external_planner/include/mpl_external_planner/ellipsoid_planner/ellipsoid_planner.h

@@ -5,8 +5,8 @@
 #ifndef MPL_ELLIPSOID_UTIL_H
 #define MPL_ELLIPSOID_UTIL_H
 #include <mpl_external_planner/ellipsoid_planner/primitive_ellipsoid_utils.h>
-
 #include <pcl/kdtree/kdtree_flann.h>
+
 #include <boost/make_shared.hpp>
 
 typedef pcl::PointXYZ PCLPoint;
@@ -18,97 +18,99 @@ typedef pcl::KdTreeFLANN<PCLPoint> KDTree;
  *
  */
 class EllipsoidUtil {
-  public:
-    /**
-     * @brief Simple constructor
-     * @param r robot radius
-     * @param h robot height, default as 0.1m
-     */
-    EllipsoidUtil(decimal_t r, decimal_t h = 0.1) { axe_ = Vec3f(r, r, h); }
-
-    ///Set obstacles
-    void setObstacles(const vec_Vec3f& obs) {
-      obs_.clear();
-      for (const auto &it : obs) {
-        if (bbox_.inside(it))
-          obs_.push_back(it);
-      }
+ public:
+  /**
+   * @brief Simple constructor
+   * @param r robot radius
+   * @param h robot height, default as 0.1m
+   */
+  EllipsoidUtil(decimal_t r, decimal_t h = 0.1) { axe_ = Vec3f(r, r, h); }
 
-      PCLPointCloud::Ptr cloud_ptr = boost::make_shared<PCLPointCloud>(toPCL(obs_));
-      kdtree_.setInputCloud(cloud_ptr);
-    }
-    ///Set bounding box
-    void setBoundingBox(const Vec3f& ori, const Vec3f& dim) {
-      Polyhedron3D Vs;
-      Vs.add(Hyperplane3D(ori + Vec3f(0, dim(1) / 2, dim(2) / 2), -Vec3f::UnitX()));
-      Vs.add(Hyperplane3D(ori + Vec3f(dim(0) / 2, 0, dim(2) / 2), -Vec3f::UnitY()));
-      Vs.add(Hyperplane3D(ori + Vec3f(dim(0) / 2, dim(2) / 2, 0), -Vec3f::UnitZ()));
-      Vs.add(Hyperplane3D(ori + dim - Vec3f(0, dim(1) / 2, dim(2) / 2), Vec3f::UnitX()));
-      Vs.add(Hyperplane3D(ori + dim - Vec3f(dim(0) / 2, 0, dim(2) / 2), Vec3f::UnitY()));
-      Vs.add(Hyperplane3D(ori + dim - Vec3f(dim(0) / 2, dim(1) / 2, 0), Vec3f::UnitZ()));
-      bbox_ = Vs;
+  /// Set obstacles
+  void setObstacles(const vec_Vec3f &obs) {
+    obs_.clear();
+    for (const auto &it : obs) {
+      if (bbox_.inside(it)) obs_.push_back(it);
     }
 
-    ///Get polyhedra
-    Polyhedron3D getBoundingBox() const {
-      return bbox_;
-    }
+    PCLPointCloud::Ptr cloud_ptr =
+        boost::make_shared<PCLPointCloud>(toPCL(obs_));
+    kdtree_.setInputCloud(cloud_ptr);
+  }
+  /// Set bounding box
+  void setBoundingBox(const Vec3f &ori, const Vec3f &dim) {
+    Polyhedron3D Vs;
+    Vs.add(
+        Hyperplane3D(ori + Vec3f(0, dim(1) / 2, dim(2) / 2), -Vec3f::UnitX()));
+    Vs.add(
+        Hyperplane3D(ori + Vec3f(dim(0) / 2, 0, dim(2) / 2), -Vec3f::UnitY()));
+    Vs.add(
+        Hyperplane3D(ori + Vec3f(dim(0) / 2, dim(2) / 2, 0), -Vec3f::UnitZ()));
+    Vs.add(Hyperplane3D(ori + dim - Vec3f(0, dim(1) / 2, dim(2) / 2),
+                        Vec3f::UnitX()));
+    Vs.add(Hyperplane3D(ori + dim - Vec3f(dim(0) / 2, 0, dim(2) / 2),
+                        Vec3f::UnitY()));
+    Vs.add(Hyperplane3D(ori + dim - Vec3f(dim(0) / 2, dim(1) / 2, 0),
+                        Vec3f::UnitZ()));
+    bbox_ = Vs;
+  }
 
-    ///Check if a primitive is inside the SFC from \f$t: 0 \rightarrow dt\f$
-    bool isFree(const Primitive3D &pr) {
-      vec_E<Waypoint3D> ps = pr.sample(2);
-      for (const auto &it : ps) {
-        if (!bbox_.inside(it.pos))
-          return false;
-      }
-      decimal_t max_v =
+  /// Get polyhedra
+  Polyhedron3D getBoundingBox() const { return bbox_; }
+
+  /// Check if a primitive is inside the SFC from \f$t: 0 \rightarrow dt\f$
+  bool isFree(const Primitive3D &pr) {
+    vec_E<Waypoint3D> ps = pr.sample(2);
+    for (const auto &it : ps) {
+      if (!bbox_.inside(it.pos)) return false;
+    }
+    decimal_t max_v =
         std::max(std::max(pr.max_vel(0), pr.max_vel(1)), pr.max_vel(2));
-      int n = std::ceil(max_v * pr.t() / axe_(0));
-      auto Es = sample_ellipsoids(pr, axe_, n);
+    int n = std::ceil(max_v * pr.t() / axe_(0));
+    auto Es = sample_ellipsoids(pr, axe_, n);
 
-      for (const auto &E : Es) {
-        float radius = axe_(0);
-        pcl::PointXYZ searchPoint;
-        std::vector<int> pointIdxRadiusSearch;
-        std::vector<float> pointRadiusSquaredDistance;
+    for (const auto &E : Es) {
+      float radius = axe_(0);
+      pcl::PointXYZ searchPoint;
+      std::vector<int> pointIdxRadiusSearch;
+      std::vector<float> pointRadiusSquaredDistance;
 
-        searchPoint.x = E.d()(0);
-        searchPoint.y = E.d()(1);
-        searchPoint.z = E.d()(2);
+      searchPoint.x = E.d()(0);
+      searchPoint.y = E.d()(1);
+      searchPoint.z = E.d()(2);
 
-        if (kdtree_.radiusSearch(searchPoint, radius, pointIdxRadiusSearch,
-                                 pointRadiusSquaredDistance) > 0) {
-          for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
-           if(E.inside(Vec3f(obs_[pointIdxRadiusSearch[i]])))
-             return false;
-        }
+      if (kdtree_.radiusSearch(searchPoint, radius, pointIdxRadiusSearch,
+                               pointRadiusSquaredDistance) > 0) {
+        for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
+          if (E.inside(Vec3f(obs_[pointIdxRadiusSearch[i]]))) return false;
       }
-
-      return true;
     }
-    ///Convert obstacle points into pcl point cloud
-    PCLPointCloud toPCL(const vec_Vec3f &obs) {
-      PCLPointCloud cloud;
-      cloud.width = obs.size();
-      cloud.height = 1;
-      cloud.points.resize(cloud.width * cloud.height);
-      for (unsigned int i = 0; i < obs.size(); i++) {
-        cloud.points[i].x = obs[i](0);
-        cloud.points[i].y = obs[i](1);
-        cloud.points[i].z = obs[i](2);
-      }
-      return cloud;
+
+    return true;
+  }
+  /// Convert obstacle points into pcl point cloud
+  PCLPointCloud toPCL(const vec_Vec3f &obs) {
+    PCLPointCloud cloud;
+    cloud.width = obs.size();
+    cloud.height = 1;
+    cloud.points.resize(cloud.width * cloud.height);
+    for (unsigned int i = 0; i < obs.size(); i++) {
+      cloud.points[i].x = obs[i](0);
+      cloud.points[i].y = obs[i](1);
+      cloud.points[i].z = obs[i](2);
     }
-  private:
-    ///robot size: axe = (r, r, h)
-    Vec3f axe_;
-    ///obstacle points
-    vec_Vec3f obs_;
-    ///obstacles in kd tree form
-    KDTree kdtree_;
-    ///Bounding box
-    Polyhedron3D bbox_;
+    return cloud;
+  }
+
+ private:
+  /// robot size: axe = (r, r, h)
+  Vec3f axe_;
+  /// obstacle points
+  vec_Vec3f obs_;
+  /// obstacles in kd tree form
+  KDTree kdtree_;
+  /// Bounding box
+  Polyhedron3D bbox_;
 };
 #endif
 
-

mpl_external_planner/include/mpl_external_planner/ellipsoid_planner/ellipsoid_util.h

@@ -14,10 +14,10 @@ namespace MPL {
  * @brief Point cloud environment
  */
 class env_cloud : public env_base<3> {
-protected:
+ protected:
   std::unique_ptr<EllipsoidUtil> map_util_;
 
-public:
+ public:
   /// Simple constructor
   env_cloud() {}
   /// Simple constructor
@@ -54,22 +54,21 @@ class env_cloud : public env_base<3> {
     succ_cost.clear();
     action_idx.clear();
 
-    //expanded_nodes_.push_back(curr.pos);
+    // expanded_nodes_.push_back(curr.pos);
     // ws_.push_back(curr);
     for (int i = 0; i < (int)U_.size(); i++) {
       Primitive3D pr(curr, U_[i], dt_);
       Waypoint3D tn = pr.evaluate(dt_);
-      if(tn == curr || !validate_primitive(pr, v_max_, a_max_, j_max_) ||
-         !map_util_->isFree(pr))
+      if (tn == curr || !validate_primitive(pr, v_max_, a_max_, j_max_) ||
+          !map_util_->isFree(pr))
         continue;
       tn.t = curr.t + dt_;
       succ.push_back(tn);
       succ_cost.push_back(pr.J(pr.control()) + w_ * dt_);
       action_idx.push_back(i);
     }
-
   }
 };
-}
+}  // namespace MPL
 
 #endif

mpl_external_planner/include/mpl_external_planner/ellipsoid_planner/env_cloud.h

@@ -4,8 +4,8 @@
  */
 #ifndef MPL_PRIMITIVE_ELLIPSOID_UTILS_H
 #define MPL_PRIMITIVE_ELLIPSOID_UTILS_H
-#include <mpl_basis/trajectory.h>
 #include <decomp_geometry/ellipsoid.h>
+#include <mpl_basis/trajectory.h>
 
 /**
  * @brief estimate the ellipsoid in 3D
@@ -14,11 +14,10 @@
  * @param acc acceleration
  */
 template <int Dim>
-Ellipsoid3D generate_ellipsoid(const Vec3f& axe,
-                             const Vecf<Dim>& pos,
-                             const Vecf<Dim>& acc) {
-  Vec3f ellipsoid_center = Dim == 2? Vec3f(pos(0), pos(1), 0) : pos;
-  Vec3f ellipsoid_acc = Dim == 2? Vec3f(acc(0), acc(1), 0) : acc;
+Ellipsoid3D generate_ellipsoid(const Vec3f& axe, const Vecf<Dim>& pos,
+                               const Vecf<Dim>& acc) {
+  Vec3f ellipsoid_center = Dim == 2 ? Vec3f(pos(0), pos(1), 0) : pos;
+  Vec3f ellipsoid_acc = Dim == 2 ? Vec3f(acc(0), acc(1), 0) : acc;
 
   const Vec3f b3 = (ellipsoid_acc + 9.81 * Vec3f::UnitZ()).normalized();
   const Vec3f bc(std::cos(0), std::sin(0), 0);
@@ -35,14 +34,14 @@ Ellipsoid3D generate_ellipsoid(const Vec3f& axe,
   return Ellipsoid3D(C, ellipsoid_center);
 }
 
-
-///Sample N+1 ellipsoids along the primitive
+/// Sample N+1 ellipsoids along the primitive
 template <int Dim>
-vec_E<Ellipsoid3D> sample_ellipsoids(const Primitive<Dim>& pr, const Vec3f& axe, int N) {
-  vec_E<Ellipsoid3D> Es(N+1);
+vec_E<Ellipsoid3D> sample_ellipsoids(const Primitive<Dim>& pr, const Vec3f& axe,
+                                     int N) {
+  vec_E<Ellipsoid3D> Es(N + 1);
   decimal_t dt = pr.t() / N;
-  for(int i = 0; i <= N; i++) {
-    const auto pt = pr.evaluate(i*dt);
+  for (int i = 0; i <= N; i++) {
+    const auto pt = pr.evaluate(i * dt);
     /*
     std::cout << "t:" << i*dt << std::endl;
     std::cout << "pos: " << pt.pos.transpose() << std::endl;
@@ -57,52 +56,52 @@ vec_E<Ellipsoid3D> sample_ellipsoids(const Primitive<Dim>& pr, const Vec3f& axe,
   return Es;
 }
 
-///Sample N ellipsoids along the trajectory
+/// Sample N ellipsoids along the trajectory
 template <int Dim>
-vec_E<Ellipsoid3D> sample_ellipsoids(const Trajectory<Dim>& traj, const Vec3f& axe, int N) {
+vec_E<Ellipsoid3D> sample_ellipsoids(const Trajectory<Dim>& traj,
+                                     const Vec3f& axe, int N) {
   vec_E<Ellipsoid3D> Es;
 
   decimal_t dt = traj.getTotalTime() / N;
-  for(decimal_t t = 0; t <= traj.getTotalTime(); t+= dt) {
+  for (decimal_t t = 0; t <= traj.getTotalTime(); t += dt) {
     Command<Dim> pt;
-    if(traj.evaluate(t, pt))
+    if (traj.evaluate(t, pt))
       Es.push_back(generate_ellipsoid<Dim>(axe, pt.pos, pt.acc));
   }
 
   return Es;
 }
 
-///Approximate the maximum roll/pitch along the trajectory
+/// Approximate the maximum roll/pitch along the trajectory
 template <int Dim>
 void max_attitude(const Trajectory<Dim>& traj, int N) {
   decimal_t dt = traj.getTotalTime() / N;
   decimal_t max_roll = 0;
   decimal_t max_roll_time = 0;
   decimal_t max_pitch = 0;
   decimal_t max_pitch_time = 0;
-  for(decimal_t t = 0; t <= traj.getTotalTime(); t+= dt) {
+  for (decimal_t t = 0; t <= traj.getTotalTime(); t += dt) {
     Command<Dim> pt;
-    if(traj.evaluate(t, pt)) {
-      const Vec3f b3 = Dim == 2 ?
-        (Vec3f(pt.acc(0), pt.acc(1), 0) +
-         9.81 * Vec3f::UnitZ()).normalized() :
-        (pt.acc + 9.81 * Vec3f::UnitZ()).normalized();
+    if (traj.evaluate(t, pt)) {
+      const Vec3f b3 =
+          Dim == 2 ? (Vec3f(pt.acc(0), pt.acc(1), 0) + 9.81 * Vec3f::UnitZ())
+                         .normalized()
+                   : (pt.acc + 9.81 * Vec3f::UnitZ()).normalized();
       decimal_t roll = std::atan2(b3(1), b3(2));
-      decimal_t pitch= std::atan2(b3(0), b3(2));
-      if(std::fabs(roll) > std::fabs(max_roll)) {
+      decimal_t pitch = std::atan2(b3(0), b3(2));
+      if (std::fabs(roll) > std::fabs(max_roll)) {
         max_roll = roll;
         max_roll_time = t;
       }
-      if(std::fabs(pitch) > std::fabs(max_pitch)) {
+      if (std::fabs(pitch) > std::fabs(max_pitch)) {
         max_pitch = pitch;
         max_pitch_time = t;
       }
-
     }
   }
 
-  printf("max roll: %f at [%f]\n", max_roll * 180/M_PI, max_roll_time);
-  printf("max pitch: %f at [%f]\n", max_pitch * 180/M_PI, max_pitch_time);
+  printf("max roll: %f at [%f]\n", max_roll * 180 / M_PI, max_roll_time);
+  printf("max pitch: %f at [%f]\n", max_pitch * 180 / M_PI, max_pitch_time);
 }
 
 #endif