From e85405786c1a763800d7198ba000c47439b7d6d4 Mon Sep 17 00:00:00 2001 From: Stewart Jamieson Date: Wed, 29 Mar 2023 16:44:26 -0400 Subject: [PATCH 1/5] fixed rmader.yaml commented parameters --- rmader/.gitignore | 2 ++ rmader/param/rmader.yaml | 6 +++--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/rmader/.gitignore b/rmader/.gitignore index c5493baa..04d1da54 100644 --- a/rmader/.gitignore +++ b/rmader/.gitignore @@ -1,3 +1,5 @@ +cmake-*/ +.idea/ *.so *.o *.lp diff --git a/rmader/param/rmader.yaml b/rmader/param/rmader.yaml index edb53a97..a3d4047e 100644 --- a/rmader/param/rmader.yaml +++ b/rmader/param/rmader.yaml @@ -77,8 +77,8 @@ setting: tuning_param: drone_bbox: [0.25, 0.25, 0.25] #(m) Used for collision checking replacing drone_radius. The bounding box dimension [x y z] Ra: 30.0 # [m] Radius of my planning sphere (planning horizon). Should be < fov_depth -############# delay_check_sec: 0.05 # seconds -############### simulated_comm_delay_sec: 0.05 #seconds + delay_check_sec: 0.05 # seconds + simulated_comm_delay_sec: 0.05 #seconds comm_delay_param: 1.0 v_max: [10.0, 10.0, 10.0] #[m/s] 7.0 a_max: [20.0, 20.0, 20.0] #[m/s2] Note that if a_max.z() > 9.81, the drone may flip @@ -177,4 +177,4 @@ nlopt: #Other possible solvers: # LD_MMA, LD_CCSAQ, LN_NELDERMEAD, LD_LBFGS, LD_VAR2, LD_AUGLAG, LD_SLSQP, LN_SBPLX, LN_PRAXIS, ... # LD_AUGLAG_EQ, LN_BOBYQA, LN_NEWUOA, LN_NEWUOA_BOUND, LN_COBYLA, LD_CCSAQ, LD_TNEWTON_PRECOND_RESTART, ... -# LD_TNEWTON_RESTART, LD_TNEWTON_PRECOND, LD_VAR1, LD_LBFGS_NOCEDAL, COBYLA \ No newline at end of file +# LD_TNEWTON_RESTART, LD_TNEWTON_PRECOND, LD_VAR1, LD_LBFGS_NOCEDAL, COBYLA From 3a2d08524acffc8d53ad660b1933563bbe98eb5e Mon Sep 17 00:00:00 2001 From: Stewart Jamieson Date: Wed, 29 Mar 2023 17:07:37 -0400 Subject: [PATCH 2/5] Rmader: made check and utility member functions const --- rmader/include/rmader.hpp | 18 +++++++++--------- rmader/src/rmader.cpp | 17 ++++++++++------- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/rmader/include/rmader.hpp b/rmader/include/rmader.hpp index 1cb5378f..ad0ad266 100644 --- a/rmader/include/rmader.hpp +++ b/rmader/include/rmader.hpp @@ -92,17 +92,17 @@ class Rmader bool initializedStateAndTermGoal(); - bool safetyCheckAfterOptForRmader(mt::PieceWisePol pwp_optimized, bool& is_q0_fail); - bool safetyCheckAfterOpt(mt::PieceWisePol pwp_optimized); + bool safetyCheckAfterOptForRmader(mt::PieceWisePol pwp_optimized, bool& is_q0_fail) const; + bool safetyCheckAfterOpt(mt::PieceWisePol pwp_optimized) const; bool safetyCheck_for_A_star_failure(mt::PieceWisePol pwp_prev); bool safetyCheck_for_A_star_failure_pwp_now(mt::PieceWisePol pwp_now); bool trajsAndPwpAreInCollision_with_inflation(mt::dynTrajCompiled traj, mt::PieceWisePol pwp_optimized, - double t_start, double t_end, bool& is_q0_fail); + double t_start, double t_end, bool& is_q0_fail) const; bool trajsAndPwpAreInCollision(mt::dynTrajCompiled traj, mt::PieceWisePol pwp_optimized, double t_start, - double t_end); + double t_end) const; void removeTrajsThatWillNotAffectMe(const mt::state& A, double t_start, double t_end); @@ -113,8 +113,8 @@ class Rmader CGAL_Polyhedron_3 convexHullOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end); std::vector vertexesOfInterval(mt::PieceWisePol& pwp, double t_start, double t_end, - const Eigen::Vector3d& delta_inflation); - std::vector vertexesOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end); + const Eigen::Vector3d& delta_inflation) const; + std::vector vertexesOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end) const; void yaw(double diff, mt::state& next_goal); void changeBBox(Eigen::Vector3d& drone_boundarybox); @@ -134,7 +134,7 @@ class Rmader mt::parameters par_; - double t_; // variable where the expressions of the trajs of the dyn obs are evaluated + mutable double t_; // variable where the expressions of the trajs of the dyn obs are evaluated std::mutex mtx_trajs_; std::vector trajs_; @@ -168,7 +168,7 @@ class Rmader std::mutex mtx_G; std::mutex mtx_G_term; - std::mutex mtx_t_; + mutable std::mutex mtx_t_; std::mutex mtx_hrtwch_; @@ -190,7 +190,7 @@ class Rmader bool exists_previous_pwp_ = false; - bool started_check_ = false; + mutable bool started_check_ = false; bool have_received_trajectories_while_checking_ = false; diff --git a/rmader/src/rmader.cpp b/rmader/src/rmader.cpp index 677c4091..0f8786a7 100644 --- a/rmader/src/rmader.cpp +++ b/rmader/src/rmader.cpp @@ -1,3 +1,5 @@ +#pragma clang diagnostic push +#pragma ide diagnostic ignored "Simplify" /* ---------------------------------------------------------------------------- * Copyright 2022, Kota Kondo, Aerospace Controls Laboratory * Massachusetts Institute of Technology @@ -351,7 +353,7 @@ void Rmader::updateTrajObstacles(mt::dynTraj traj) } std::vector Rmader::vertexesOfInterval(mt::PieceWisePol& pwp, double t_start, double t_end, - const Eigen::Vector3d& delta) + const Eigen::Vector3d& delta) const { std::vector points; @@ -422,7 +424,7 @@ std::vector Rmader::vertexesOfInterval(mt::PieceWisePol& pwp, d } // return a vector that contains all the vertexes of the polyhedral approx of an interval. -std::vector Rmader::vertexesOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end) +std::vector Rmader::vertexesOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end) const { Eigen::Vector3d delta = Eigen::Vector3d::Zero(); Eigen::Vector3d drone_boundarybox = par_.drone_bbox; @@ -914,7 +916,7 @@ std::vector Rmader::getTrajs() // check wheter a mt::dynTrajCompiled and a pwp_optimized are in collision in the interval [t_start, t_end] bool Rmader::trajsAndPwpAreInCollision_with_inflation(mt::dynTrajCompiled traj, mt::PieceWisePol pwp_optimized, - double t_start, double t_end, bool& is_q0_fail) + double t_start, double t_end, bool& is_q0_fail) const { Eigen::Vector3d n_i; double d_i; @@ -962,7 +964,7 @@ bool Rmader::trajsAndPwpAreInCollision_with_inflation(mt::dynTrajCompiled traj, } bool Rmader::trajsAndPwpAreInCollision(mt::dynTrajCompiled traj, mt::PieceWisePol pwp_optimized, double t_start, - double t_end) + double t_end) const { Eigen::Vector3d n_i; double d_i; @@ -1001,7 +1003,7 @@ bool Rmader::trajsAndPwpAreInCollision(mt::dynTrajCompiled traj, mt::PieceWisePo // Checks that I have not received new trajectories that affect me while doing the optimization // Check period and Recheck period is defined here -bool Rmader::safetyCheckAfterOptForRmader(mt::PieceWisePol pwp_optimized, bool& is_q0_fail) +bool Rmader::safetyCheckAfterOptForRmader(mt::PieceWisePol pwp_optimized, bool& is_q0_fail) const { bool result = true; for (auto& traj : trajs_) @@ -1022,7 +1024,7 @@ bool Rmader::safetyCheckAfterOptForRmader(mt::PieceWisePol pwp_optimized, bool& } // Checks that I have not received new trajectories that affect me while doing the optimization -bool Rmader::safetyCheckAfterOpt(mt::PieceWisePol pwp_optimized) +bool Rmader::safetyCheckAfterOpt(mt::PieceWisePol pwp_optimized) const { started_check_ = true; @@ -2448,4 +2450,5 @@ void Rmader::printDroneStatus() void Rmader::getID(int& id) { id_ = id; -} \ No newline at end of file +} +#pragma clang diagnostic pop \ No newline at end of file From 7d1b7b8e31acd4162fe88b4463e4b70d3a16ce30 Mon Sep 17 00:00:00 2001 From: Stewart Jamieson Date: Wed, 29 Mar 2023 17:14:12 -0400 Subject: [PATCH 3/5] rmader: const loop iterators and more const member functions --- rmader/include/rmader.hpp | 12 ++++++------ rmader/src/rmader.cpp | 31 ++++++++++++++----------------- 2 files changed, 20 insertions(+), 23 deletions(-) diff --git a/rmader/include/rmader.hpp b/rmader/include/rmader.hpp index ad0ad266..12600316 100644 --- a/rmader/include/rmader.hpp +++ b/rmader/include/rmader.hpp @@ -45,7 +45,7 @@ using namespace termcolor; class Rmader { public: - Rmader(mt::parameters par); + Rmader(mt::parameters const& par); bool replan(mt::Edges& edges_obstacles_out, std::vector& X_safe_out, std::vector& planes, int& num_of_LPs_run, int& num_of_QCQPs_run, mt::PieceWisePol& pwp_out); bool replan_with_delaycheck(mt::Edges& edges_obstacles_out, std::vector& headsup_plan, @@ -64,17 +64,17 @@ class Rmader void setTerminalGoal(mt::state& term_goal); void resetInitialization(); - bool IsTranslating(); - void updateTrajObstacles_with_delaycheck(mt::dynTraj traj); - void updateTrajObstacles(mt::dynTraj traj); + bool IsTranslating() const; + void updateTrajObstacles_with_delaycheck(mt::dynTraj const& traj); + void updateTrajObstacles(mt::dynTraj const& traj); Eigen::Vector2d RotationMatrix(Eigen::Vector2d& vec, const double& angle); void getID(int& id); mt::state moveAoutOfBbox(const mt::state& A); int getMissedMsgsCnt(); mt::state getGterm(); - bool isGoalSeen(); - bool isGoalReached(); + bool isGoalSeen() const; + bool isGoalReached() const; bool initializedAllExceptPlanner(); std::vector getTrajs(); diff --git a/rmader/src/rmader.cpp b/rmader/src/rmader.cpp index 0f8786a7..39122948 100644 --- a/rmader/src/rmader.cpp +++ b/rmader/src/rmader.cpp @@ -1,5 +1,3 @@ -#pragma clang diagnostic push -#pragma ide diagnostic ignored "Simplify" /* ---------------------------------------------------------------------------- * Copyright 2022, Kota Kondo, Aerospace Controls Laboratory * Massachusetts Institute of Technology @@ -31,7 +29,7 @@ using namespace termcolor; // typedef ROSWallTimer MyTimer; typedef RMADER_timers::Timer MyTimer; -Rmader::Rmader(mt::parameters par) : par_(par) +Rmader::Rmader(mt::parameters const& par) : par_(par) { // drone_status_ == DroneStatus::YAWING; drone_status_ == DroneStatus::TRAVELING; @@ -135,7 +133,7 @@ Rmader::Rmader(mt::parameters par) : par_(par) void Rmader::dynTraj2dynTrajCompiled(const mt::dynTraj& traj, mt::dynTrajCompiled& traj_compiled) { mtx_t_.lock(); - for (auto function_i : traj.function) + for (auto const& function_i : traj.function) { typedef exprtk::symbol_table symbol_table_t; typedef exprtk::expression expression_t; @@ -174,7 +172,7 @@ void Rmader::dynTraj2dynTrajCompiled(const mt::dynTraj& traj, mt::dynTrajCompile } // Note that we need to compile the trajectories inside mader.cpp because t_ is in mader.hpp -void Rmader::updateTrajObstacles_with_delaycheck(mt::dynTraj traj) +void Rmader::updateTrajObstacles_with_delaycheck(mt::dynTraj const& traj) { // std::cout << "in updateTrajObstacles" << std::endl; @@ -216,7 +214,7 @@ void Rmader::updateTrajObstacles_with_delaycheck(mt::dynTraj traj) std::vector local_trajs; - for (auto traj_compiled : trajs_) + for (auto const& traj_compiled : trajs_) { mtx_t_.lock(); t_ = ros::Time::now().toSec(); @@ -273,7 +271,7 @@ void Rmader::updateTrajObstacles_with_delaycheck(mt::dynTraj traj) // std::cout << "aft mtx_trajs_.unlock() in updateTrajObstacles" << std::endl; } -void Rmader::updateTrajObstacles(mt::dynTraj traj) +void Rmader::updateTrajObstacles(mt::dynTraj const& traj) { MyTimer tmp_t(true); @@ -307,7 +305,7 @@ void Rmader::updateTrajObstacles(mt::dynTraj traj) // Note that these positions are obtained with the trajectory stored in the past in the local map std::vector local_trajs; - for (auto traj_compiled : trajs_) + for (auto const& traj_compiled : trajs_) { mtx_t_.lock(); t_ = ros::Time::now().toSec(); @@ -667,7 +665,7 @@ void Rmader::removeTrajsThatWillNotAffectMe(const mt::state& A, double t_start, }*/ } -bool Rmader::IsTranslating() +bool Rmader::IsTranslating() const { return (drone_status_ == DroneStatus::GOAL_SEEN || drone_status_ == DroneStatus::TRAVELING); } @@ -1029,7 +1027,7 @@ bool Rmader::safetyCheckAfterOpt(mt::PieceWisePol pwp_optimized) const started_check_ = true; bool result = true; - for (auto traj : trajs_) + for (auto const& traj : trajs_) { if (traj.time_received > time_init_opt_ && traj.is_agent == true) { @@ -1060,7 +1058,7 @@ bool Rmader::delayCheck(mt::PieceWisePol pwp_now, const double& headsup_time) bool is_q0_fail = false; mtx_trajs_.lock(); // this function is called in mader_ros.cpp so need to lock in the function bool result = true; - for (auto& traj_compiled : trajs_) + for (auto const& traj_compiled : trajs_) { if (traj_compiled.is_agent == true) { @@ -1150,7 +1148,7 @@ bool Rmader::delayCheck(mt::PieceWisePol pwp_now, const double& headsup_time) bool Rmader::safetyCheck_for_A_star_failure(mt::PieceWisePol pwp_prev) { bool result = true; - for (auto traj : trajs_) + for (auto const& traj : trajs_) { if (traj.time_received > time_init_opt_ && traj.is_agent == true) { @@ -1173,7 +1171,7 @@ bool Rmader::safetyCheck_for_A_star_failure(mt::PieceWisePol pwp_prev) bool Rmader::safetyCheck_for_A_star_failure_pwp_now(mt::PieceWisePol pwp_now) { bool result = true; - for (auto traj : trajs_) + for (auto const& traj : trajs_) { if (traj.time_received > time_init_opt_ && traj.is_agent == true) { @@ -1241,7 +1239,7 @@ bool Rmader::isReplanningNeeded() return true; } -bool Rmader::isGoalSeen() +bool Rmader::isGoalSeen() const { if (drone_status_ == DroneStatus::GOAL_SEEN) { @@ -1253,7 +1251,7 @@ bool Rmader::isGoalSeen() } } -bool Rmader::isGoalReached() +bool Rmader::isGoalReached() const { if (drone_status_ == DroneStatus::GOAL_REACHED) { @@ -2450,5 +2448,4 @@ void Rmader::printDroneStatus() void Rmader::getID(int& id) { id_ = id; -} -#pragma clang diagnostic pop \ No newline at end of file +} \ No newline at end of file From 7714bb969ab3ced95221e0d5157764500c8d48d8 Mon Sep 17 00:00:00 2001 From: Stewart Jamieson Date: Wed, 29 Mar 2023 17:18:13 -0400 Subject: [PATCH 4/5] rmader: replaced push_back Minkowski sums with emplace_back --- rmader/src/rmader.cpp | 54 +++++++++++++++++++++---------------------- 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/rmader/src/rmader.cpp b/rmader/src/rmader.cpp index 39122948..776d8864 100644 --- a/rmader/src/rmader.cpp +++ b/rmader/src/rmader.cpp @@ -393,7 +393,7 @@ std::vector Rmader::vertexesOfInterval(mt::PieceWisePol& pwp, d if (delta.norm() < 1e-6) { // no inflation - points.push_back(Eigen::Vector3d(x, y, z)); + points.emplace_back(x, y, z); } else { @@ -401,18 +401,18 @@ std::vector Rmader::vertexesOfInterval(mt::PieceWisePol& pwp, d if (j == V.cols() - 1) { - points.push_back(Eigen::Vector3d(x + delta.x(), y + delta.y(), z + delta.z())); - points.push_back(Eigen::Vector3d(x + delta.x(), y - delta.y(), z - delta.z())); - points.push_back(Eigen::Vector3d(x + delta.x(), y + delta.y(), z - delta.z())); - points.push_back(Eigen::Vector3d(x + delta.x(), y - delta.y(), z + delta.z())); - points.push_back(Eigen::Vector3d(x - delta.x(), y - delta.y(), z - delta.z())); - points.push_back(Eigen::Vector3d(x - delta.x(), y + delta.y(), z + delta.z())); - points.push_back(Eigen::Vector3d(x - delta.x(), y + delta.y(), z - delta.z())); - points.push_back(Eigen::Vector3d(x - delta.x(), y - delta.y(), z + delta.z())); + points.emplace_back(x + delta.x(), y + delta.y(), z + delta.z()); + points.emplace_back(x + delta.x(), y - delta.y(), z - delta.z()); + points.emplace_back(x + delta.x(), y + delta.y(), z - delta.z()); + points.emplace_back(x + delta.x(), y - delta.y(), z + delta.z()); + points.emplace_back(x - delta.x(), y - delta.y(), z - delta.z()); + points.emplace_back(x - delta.x(), y + delta.y(), z + delta.z()); + points.emplace_back(x - delta.x(), y + delta.y(), z - delta.z()); + points.emplace_back(x - delta.x(), y - delta.y(), z + delta.z()); } else { - points.push_back(Eigen::Vector3d(x, y, z)); + points.emplace_back(x, y, z); } } } @@ -457,14 +457,14 @@ std::vector Rmader::vertexesOfInterval(mt::dynTrajCompiled& tra mtx_t_.unlock(); //"Minkowski sum along the trajectory: box centered on the trajectory" - points.push_back(Eigen::Vector3d(x + delta.x(), y + delta.y(), z + delta.z())); - points.push_back(Eigen::Vector3d(x + delta.x(), y - delta.y(), z - delta.z())); - points.push_back(Eigen::Vector3d(x + delta.x(), y + delta.y(), z - delta.z())); - points.push_back(Eigen::Vector3d(x + delta.x(), y - delta.y(), z + delta.z())); - points.push_back(Eigen::Vector3d(x - delta.x(), y - delta.y(), z - delta.z())); - points.push_back(Eigen::Vector3d(x - delta.x(), y + delta.y(), z + delta.z())); - points.push_back(Eigen::Vector3d(x - delta.x(), y + delta.y(), z - delta.z())); - points.push_back(Eigen::Vector3d(x - delta.x(), y - delta.y(), z + delta.z())); + points.emplace_back(x + delta.x(), y + delta.y(), z + delta.z()); + points.emplace_back(x + delta.x(), y - delta.y(), z - delta.z()); + points.emplace_back(x + delta.x(), y + delta.y(), z - delta.z()); + points.emplace_back(x + delta.x(), y - delta.y(), z + delta.z()); + points.emplace_back(x - delta.x(), y - delta.y(), z - delta.z()); + points.emplace_back(x - delta.x(), y + delta.y(), z + delta.z()); + points.emplace_back(x - delta.x(), y + delta.y(), z - delta.z()); + points.emplace_back(x - delta.x(), y - delta.y(), z + delta.z()); } return points; @@ -560,7 +560,7 @@ CGAL_Polyhedron_3 Rmader::convexHullOfInterval(mt::dynTrajCompiled& traj, double std::vector points_cgal; for (auto point_i : points) { - points_cgal.push_back(Point_3(point_i.x(), point_i.y(), point_i.z())); + points_cgal.emplace_back(point_i.x(), point_i.y(), point_i.z()); } // std::cout << "1.1.6" << std::endl; @@ -576,14 +576,14 @@ void Rmader::removeTrajsThatWillNotAffectMe(const mt::state& A, double t_start, std::vector pointsB; Eigen::Vector3d delta = par_.Ra * Eigen::Vector3d::Ones(); - pointsB.push_back(Eigen::Vector3d(A.pos.x() + delta.x(), A.pos.y() + delta.y(), A.pos.z() + delta.z())); - pointsB.push_back(Eigen::Vector3d(A.pos.x() + delta.x(), A.pos.y() - delta.y(), A.pos.z() - delta.z())); - pointsB.push_back(Eigen::Vector3d(A.pos.x() + delta.x(), A.pos.y() + delta.y(), A.pos.z() - delta.z())); - pointsB.push_back(Eigen::Vector3d(A.pos.x() + delta.x(), A.pos.y() - delta.y(), A.pos.z() + delta.z())); - pointsB.push_back(Eigen::Vector3d(A.pos.x() - delta.x(), A.pos.y() - delta.y(), A.pos.z() - delta.z())); - pointsB.push_back(Eigen::Vector3d(A.pos.x() - delta.x(), A.pos.y() + delta.y(), A.pos.z() + delta.z())); - pointsB.push_back(Eigen::Vector3d(A.pos.x() - delta.x(), A.pos.y() + delta.y(), A.pos.z() - delta.z())); - pointsB.push_back(Eigen::Vector3d(A.pos.x() - delta.x(), A.pos.y() - delta.y(), A.pos.z() + delta.z())); + pointsB.emplace_back(A.pos.x() + delta.x(), A.pos.y() + delta.y(), A.pos.z() + delta.z()); + pointsB.emplace_back(A.pos.x() + delta.x(), A.pos.y() - delta.y(), A.pos.z() - delta.z()); + pointsB.emplace_back(A.pos.x() + delta.x(), A.pos.y() + delta.y(), A.pos.z() - delta.z()); + pointsB.emplace_back(A.pos.x() + delta.x(), A.pos.y() - delta.y(), A.pos.z() + delta.z()); + pointsB.emplace_back(A.pos.x() - delta.x(), A.pos.y() - delta.y(), A.pos.z() - delta.z()); + pointsB.emplace_back(A.pos.x() - delta.x(), A.pos.y() + delta.y(), A.pos.z() + delta.z()); + pointsB.emplace_back(A.pos.x() - delta.x(), A.pos.y() + delta.y(), A.pos.z() - delta.z()); + pointsB.emplace_back(A.pos.x() - delta.x(), A.pos.y() - delta.y(), A.pos.z() + delta.z()); std::vector local_trajs; From fd1c9d072f45b8ae531085bc449ceb67772ccb3b Mon Sep 17 00:00:00 2001 From: Stewart Jamieson Date: Wed, 29 Mar 2023 17:52:01 -0400 Subject: [PATCH 5/5] rmader: pre-allocated space for convex hulls of curves, convex hulls of intervals, and vertices of intervals --- rmader/include/rmader.hpp | 8 ++++---- rmader/src/rmader.cpp | 18 +++++++++++------- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/rmader/include/rmader.hpp b/rmader/include/rmader.hpp index 12600316..38c46f7a 100644 --- a/rmader/include/rmader.hpp +++ b/rmader/include/rmader.hpp @@ -109,12 +109,12 @@ class Rmader /* vec_E> cu::vectorGCALPol2vectorJPSPol(ConvexHullsOfCurves& convex_hulls_of_curves); mt::ConvexHullsOfCurves_Std cu::vectorGCALPol2vectorStdEigen(ConvexHullsOfCurves& convexHulls);*/ ConvexHullsOfCurves convexHullsOfCurves(double t_start, double t_end); - ConvexHullsOfCurve convexHullsOfCurve(mt::dynTrajCompiled& traj, double t_start, double t_end); - CGAL_Polyhedron_3 convexHullOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end); + ConvexHullsOfCurve convexHullsOfCurve(mt::dynTrajCompiled const& traj, double t_start, double t_end); + CGAL_Polyhedron_3 convexHullOfInterval(mt::dynTrajCompiled const& traj, double t_start, double t_end); - std::vector vertexesOfInterval(mt::PieceWisePol& pwp, double t_start, double t_end, + std::vector vertexesOfInterval(mt::PieceWisePol const& pwp, double t_start, double t_end, const Eigen::Vector3d& delta_inflation) const; - std::vector vertexesOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end) const; + std::vector vertexesOfInterval(mt::dynTrajCompiled const& traj, double t_start, double t_end) const; void yaw(double diff, mt::state& next_goal); void changeBBox(Eigen::Vector3d& drone_boundarybox); diff --git a/rmader/src/rmader.cpp b/rmader/src/rmader.cpp index 776d8864..b6418014 100644 --- a/rmader/src/rmader.cpp +++ b/rmader/src/rmader.cpp @@ -350,13 +350,13 @@ void Rmader::updateTrajObstacles(mt::dynTraj const& traj) // std::cout << bold << blue << "updateTrajObstacles took " << tmp_t << reset << std::endl; } -std::vector Rmader::vertexesOfInterval(mt::PieceWisePol& pwp, double t_start, double t_end, +std::vector Rmader::vertexesOfInterval(mt::PieceWisePol const& pwp, double t_start, double t_end, const Eigen::Vector3d& delta) const { std::vector points; - std::vector::iterator low = std::lower_bound(pwp.times.begin(), pwp.times.end(), t_start); - std::vector::iterator up = std::upper_bound(pwp.times.begin(), pwp.times.end(), t_end); + auto const low = std::lower_bound(pwp.times.begin(), pwp.times.end(), t_start); + auto const up = std::upper_bound(pwp.times.begin(), pwp.times.end(), t_end); // // Example: times=[1 2 3 4 5 6 7] // // t_start=1.5; @@ -422,7 +422,7 @@ std::vector Rmader::vertexesOfInterval(mt::PieceWisePol& pwp, d } // return a vector that contains all the vertexes of the polyhedral approx of an interval. -std::vector Rmader::vertexesOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end) const +std::vector Rmader::vertexesOfInterval(mt::dynTrajCompiled const& traj, double t_start, double t_end) const { Eigen::Vector3d delta = Eigen::Vector3d::Zero(); Eigen::Vector3d drone_boundarybox = par_.drone_bbox; @@ -430,6 +430,8 @@ std::vector Rmader::vertexesOfInterval(mt::dynTrajCompiled& tra if (traj.is_agent == false) { std::vector points; + long const estimated_size = (t_end - t_start) / par_.gamma; + if (estimated_size > 0) points.reserve(8u * (1 + static_cast(estimated_size))); // delta = traj.bbox / 2.0 + (par_.drone_radius + par_.beta + par_.alpha) * // Eigen::Vector3d::Ones(); // every side of the box will be increased by 2*delta //(+delta on one end, -delta on the other) @@ -549,7 +551,7 @@ void Rmader::changeBBox(Eigen::Vector3d& drone_boundarybox) } // See https://doc.cgal.org/Manual/3.7/examples/Convex_hull_3/quickhull_3.cpp -CGAL_Polyhedron_3 Rmader::convexHullOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end) +CGAL_Polyhedron_3 Rmader::convexHullOfInterval(mt::dynTrajCompiled const& traj, double t_start, double t_end) { // std::cout << "1.1.4" << std::endl; @@ -670,9 +672,10 @@ bool Rmader::IsTranslating() const return (drone_status_ == DroneStatus::GOAL_SEEN || drone_status_ == DroneStatus::TRAVELING); } -ConvexHullsOfCurve Rmader::convexHullsOfCurve(mt::dynTrajCompiled& traj, double t_start, double t_end) +ConvexHullsOfCurve Rmader::convexHullsOfCurve(mt::dynTrajCompiled const& traj, double t_start, double t_end) { ConvexHullsOfCurve convexHulls; + convexHulls.reserve(par_.num_pol); double deltaT = (t_end - t_start) / (1.0 * par_.num_pol); // num_pol is the number of intervals // std::cout << "1.1.3" << std::endl; @@ -688,10 +691,11 @@ ConvexHullsOfCurve Rmader::convexHullsOfCurve(mt::dynTrajCompiled& traj, double ConvexHullsOfCurves Rmader::convexHullsOfCurves(double t_start, double t_end) { ConvexHullsOfCurves result; + result.reserve(trajs_.size()); // std::cout << "1.1.2" << std::endl; - for (auto traj : trajs_) + for (auto const& traj : trajs_) { result.push_back(convexHullsOfCurve(traj, t_start, t_end)); }