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/CMakeLists.txt b/rmader/CMakeLists.txt index a893a32d..fc5fea57 100644 --- a/rmader/CMakeLists.txt +++ b/rmader/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(rmader) ## Add support for C++11, supported in ROS Kinetic and newer -add_definitions(-std=c++11) +add_definitions(-std=c++17) #add_definitions(-std=c99) diff --git a/rmader/include/rmader.hpp b/rmader/include/rmader.hpp index 1cb5378f..950675a6 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(); @@ -92,29 +92,29 @@ 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); /* 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, - const Eigen::Vector3d& delta_inflation); - std::vector vertexesOfInterval(mt::dynTrajCompiled& traj, 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 const& traj, double t_start, double t_end) const; void yaw(double diff, mt::state& next_goal); void changeBBox(Eigen::Vector3d& drone_boundarybox); @@ -132,9 +132,16 @@ class Rmader void printDroneStatus(); + template + auto evaluateAt(double const time, F&& f) const { + std::lock_guard guard(mtx_t_); + t_ = time; + return std::forward(f)(); + } + 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 +175,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 +197,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/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 diff --git a/rmader/src/rmader.cpp b/rmader/src/rmader.cpp index 677c4091..fbcc45de 100644 --- a/rmader/src/rmader.cpp +++ b/rmader/src/rmader.cpp @@ -23,13 +23,14 @@ #endif using namespace termcolor; +using mtx_guard = std::lock_guard; // Uncomment the type of timer you want: // typedef ROSTimer MyTimer;getNextGoal // 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; @@ -132,8 +133,8 @@ 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) + mtx_guard guard(mtx_t_); + for (auto const& function_i : traj.function) { typedef exprtk::symbol_table symbol_table_t; typedef exprtk::expression expression_t; @@ -151,8 +152,6 @@ void Rmader::dynTraj2dynTrajCompiled(const mt::dynTraj& traj, mt::dynTrajCompile traj_compiled.function.push_back(expression); } - mtx_t_.unlock(); - traj_compiled.bbox = traj.bbox; traj_compiled.id = traj.id; traj_compiled.time_received = traj.time_received; // ros::Time::now().toSec(); @@ -172,7 +171,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; @@ -214,16 +213,14 @@ 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(); - - Eigen::Vector3d center_obs; - center_obs << traj_compiled.function[0].value(), traj_compiled.function[1].value(), - traj_compiled.function[2].value(); - - mtx_t_.unlock(); + auto const& center_obs = evaluateAt(ros::Time::now().toSec(), [&](){ + Eigen::Vector3d center_obs; + center_obs << traj_compiled.function[0].value(), traj_compiled.function[1].value(), + traj_compiled.function[2].value(); + return center_obs; + }); if (((traj_compiled.is_static == true) && (center_obs - state_.pos).norm() > 2 * par_.Ra) || //// ((traj_compiled.is_static == false) && (center_obs - state_.pos).norm() > 4 * par_.Ra)) @@ -271,7 +268,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); @@ -305,16 +302,14 @@ 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(); - - Eigen::Vector3d center_obs; - center_obs << traj_compiled.function[0].value(), traj_compiled.function[1].value(), - traj_compiled.function[2].value(); - - mtx_t_.unlock(); + auto const& center_obs = evaluateAt(ros::Time::now().toSec(), [&](){ + Eigen::Vector3d center_obs; + center_obs << traj_compiled.function[0].value(), traj_compiled.function[1].value(), + traj_compiled.function[2].value(); + return center_obs; + }); if (((traj_compiled.is_static == true) && (center_obs - state_.pos).norm() > 2 * par_.Ra) || //// ((traj_compiled.is_static == false) && (center_obs - state_.pos).norm() > 4 * par_.Ra)) @@ -350,13 +345,13 @@ void Rmader::updateTrajObstacles(mt::dynTraj 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, - const Eigen::Vector3d& delta) +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; @@ -393,7 +388,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 +396,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); } } } @@ -422,7 +417,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 const& traj, double t_start, double t_end) const { Eigen::Vector3d delta = Eigen::Vector3d::Zero(); Eigen::Vector3d drone_boundarybox = par_.drone_bbox; @@ -430,6 +425,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 && estimated_size < 1000000000) 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) @@ -448,23 +445,21 @@ std::vector Rmader::vertexesOfInterval(mt::dynTrajCompiled& tra ((t > t_end) && ((t - t_end) < par_.gamma)); /////// This is to ensure we have a sample a the end t = t + par_.gamma) { - mtx_t_.lock(); - t_ = std::min(t, t_end); // this min only has effect on the last sample - - double x = traj.function[0].value(); - double y = traj.function[1].value(); - double z = traj.function[2].value(); - mtx_t_.unlock(); + auto const& [x, y, z] = evaluateAt(std::min(t, t_end), [&](){ + return std::make_tuple(traj.function[0].value(), + traj.function[1].value(), + traj.function[2].value()); + }); //"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; @@ -549,7 +544,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; @@ -560,7 +555,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 +571,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; @@ -594,23 +589,22 @@ void Rmader::removeTrajsThatWillNotAffectMe(const mt::state& A, double t_start, // STATIC OBSTACLES/AGENTS if (traj.is_static == true) { - mtx_t_.lock(); - t_ = t_start; // which is constant along the trajectory - - Eigen::Vector3d center_obs; - if (traj.is_agent == false) - { - center_obs << traj.function[0].value(), traj.function[1].value(), traj.function[2].value(); - } - else - { - center_obs = traj.pwp.eval(t_); - } + auto const& [center_obs, positive_half_diagonal] = evaluateAt(t_start, [&](){ + Eigen::Vector3d center_obs; + // TODO: should mtx_t_ be locked here? And what is t_ set to? + if (traj.is_agent == false) + { + center_obs << traj.function[0].value(), traj.function[1].value(), traj.function[2].value(); + } + else + { + center_obs = traj.pwp.eval(t_); + } - mtx_t_.unlock(); - // mtx_t_.unlock(); - Eigen::Vector3d positive_half_diagonal; - positive_half_diagonal << traj.bbox[0] / 2.0, traj.bbox[1] / 2.0, traj.bbox[2] / 2.0; + Eigen::Vector3d positive_half_diagonal; + positive_half_diagonal << traj.bbox[0] / 2.0, traj.bbox[1] / 2.0, traj.bbox[2] / 2.0; + return std::make_tuple(center_obs, positive_half_diagonal); + }); Eigen::Vector3d c1 = center_obs - positive_half_diagonal; Eigen::Vector3d c2 = center_obs + positive_half_diagonal; @@ -665,14 +659,15 @@ 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); } -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 +683,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)); } @@ -914,7 +910,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 +958,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 +997,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,12 +1018,12 @@ 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; bool result = true; - for (auto traj : trajs_) + for (auto const& traj : trajs_) { if (traj.time_received > time_init_opt_ && traj.is_agent == true) { @@ -1058,7 +1054,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) { @@ -1148,7 +1144,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) { @@ -1171,7 +1167,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) { @@ -1239,7 +1235,7 @@ bool Rmader::isReplanningNeeded() return true; } -bool Rmader::isGoalSeen() +bool Rmader::isGoalSeen() const { if (drone_status_ == DroneStatus::GOAL_SEEN) { @@ -1251,7 +1247,7 @@ bool Rmader::isGoalSeen() } } -bool Rmader::isGoalReached() +bool Rmader::isGoalReached() const { if (drone_status_ == DroneStatus::GOAL_REACHED) {