From 2d98a183dce60bf3f0a67361631b259f9bacfe15 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Wed, 17 Apr 2024 15:57:46 +0200 Subject: [PATCH 01/10] Trigger coverage with a button --- .../mav_planning_rviz/planning_panel.h | 1 + mav_planning_rviz/src/planning_panel.cpp | 21 +++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h index 50c743b0..be1d8381 100644 --- a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h +++ b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h @@ -76,6 +76,7 @@ class PlanningPanel : public rviz::Panel { void setPlannerModeServiceRollout(); void setPlannerModeServiceAbort(); void setPlannerModeServiceReturn(); + void setPlannerModeServiceCoverage(); protected: // Set up the layout, only called by the constructor. diff --git a/mav_planning_rviz/src/planning_panel.cpp b/mav_planning_rviz/src/planning_panel.cpp index 89f91928..4de3ed54 100644 --- a/mav_planning_rviz/src/planning_panel.cpp +++ b/mav_planning_rviz/src/planning_panel.cpp @@ -73,17 +73,20 @@ QGroupBox* PlanningPanel::createPlannerModeGroup() { set_planner_state_buttons_.push_back(new QPushButton("ROLLOUT")); set_planner_state_buttons_.push_back(new QPushButton("ABORT")); set_planner_state_buttons_.push_back(new QPushButton("RETURN")); + set_planner_state_buttons_.push_back(new QPushButton("COVERAGE")); service_layout->addWidget(set_planner_state_buttons_[0], 0, 0, 1, 1); service_layout->addWidget(set_planner_state_buttons_[1], 0, 1, 1, 1); service_layout->addWidget(set_planner_state_buttons_[3], 0, 2, 1, 1); service_layout->addWidget(set_planner_state_buttons_[2], 0, 3, 1, 1); + service_layout->addWidget(set_planner_state_buttons_[4], 0, 4, 1, 1); groupBox->setLayout(service_layout); connect(set_planner_state_buttons_[0], SIGNAL(released()), this, SLOT(setPlannerModeServiceNavigate())); connect(set_planner_state_buttons_[1], SIGNAL(released()), this, SLOT(setPlannerModeServiceRollout())); connect(set_planner_state_buttons_[2], SIGNAL(released()), this, SLOT(setPlannerModeServiceAbort())); connect(set_planner_state_buttons_[3], SIGNAL(released()), this, SLOT(setPlannerModeServiceReturn())); + connect(set_planner_state_buttons_[4], SIGNAL(released()), this, SLOT(setPlannerModeServiceCoverage())); return groupBox; } @@ -504,6 +507,11 @@ void PlanningPanel::setPlannerModeServiceRollout() { callSetPlannerStateService("/terrain_planner/set_planner_state", 3); } +void PlanningPanel::setPlannerModeServiceCoverage() { + callSetPlannerStateService("/terrain_planner/set_planner_state", 6); +} + + void PlanningPanel::callSetPlannerStateService(std::string service_name, const int mode) { std::thread t([service_name, mode] { planner_msgs::SetPlannerState req; @@ -615,6 +623,7 @@ void PlanningPanel::plannerstateCallback(const planner_msgs::NavigationStatus& m set_planner_state_buttons_[1]->setDisabled(false); // ROLLOUT set_planner_state_buttons_[2]->setDisabled(true); // ABORT set_planner_state_buttons_[3]->setDisabled(false); // RETURN + set_planner_state_buttons_[4]->setDisabled(false); // Coverage break; } case PLANNER_STATE::NAVIGATE: { @@ -622,6 +631,7 @@ void PlanningPanel::plannerstateCallback(const planner_msgs::NavigationStatus& m set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT set_planner_state_buttons_[2]->setDisabled(false); // ABORT set_planner_state_buttons_[3]->setDisabled(true); // RETURN + set_planner_state_buttons_[4]->setDisabled(true); // Coverage break; } case PLANNER_STATE::ROLLOUT: { @@ -629,6 +639,7 @@ void PlanningPanel::plannerstateCallback(const planner_msgs::NavigationStatus& m set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT set_planner_state_buttons_[2]->setDisabled(false); // ABORT set_planner_state_buttons_[3]->setDisabled(true); // RETURN + set_planner_state_buttons_[4]->setDisabled(true); // Coverage break; } case PLANNER_STATE::ABORT: { @@ -636,6 +647,7 @@ void PlanningPanel::plannerstateCallback(const planner_msgs::NavigationStatus& m set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT set_planner_state_buttons_[2]->setDisabled(true); // ABORT set_planner_state_buttons_[3]->setDisabled(true); // RETURN + set_planner_state_buttons_[4]->setDisabled(true); // Coverage break; } case PLANNER_STATE::RETURN: { @@ -643,6 +655,15 @@ void PlanningPanel::plannerstateCallback(const planner_msgs::NavigationStatus& m set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT set_planner_state_buttons_[2]->setDisabled(false); // ABORT set_planner_state_buttons_[3]->setDisabled(true); // RETURN + set_planner_state_buttons_[4]->setDisabled(true); // Coverage + break; + } + case PLANNER_STATE::COVERAGE: { + set_planner_state_buttons_[0]->setDisabled(true); // NAVIGATE + set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT + set_planner_state_buttons_[2]->setDisabled(false); // ABORT + set_planner_state_buttons_[3]->setDisabled(true); // RETURN + set_planner_state_buttons_[4]->setDisabled(true); // Coverage break; } } From 456db9f01bb8f0bb1750ceb6f15089e95232a4ad Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Wed, 17 Apr 2024 17:12:10 +0200 Subject: [PATCH 02/10] Add coverage --- mav_planning_rviz/include/mav_planning_rviz/planning_panel.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h index be1d8381..d4b370ea 100644 --- a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h +++ b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h @@ -13,7 +13,7 @@ #include "planner_msgs/NavigationStatus.h" #endif -enum PLANNER_STATE { HOLD = 1, NAVIGATE = 2, ROLLOUT = 3, ABORT = 4, RETURN = 5 }; +enum PLANNER_STATE { HOLD = 1, NAVIGATE = 2, ROLLOUT = 3, ABORT = 4, RETURN = 5, COVERAGE = 6 }; class QLineEdit; class QCheckBox; From 48c70b3ec6de1f815e28bc5a856d08b85f679d50 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Wed, 17 Apr 2024 18:23:56 +0200 Subject: [PATCH 03/10] Add coverage button --- mav_planning_rviz/src/planning_panel.cpp | 13 ++++--- .../terrain_planner/terrain_ompl_rrt.h | 10 ++++++ terrain_planner/src/terrain_ompl_rrt.cpp | 36 +++++++++++++++++++ 3 files changed, 52 insertions(+), 7 deletions(-) diff --git a/mav_planning_rviz/src/planning_panel.cpp b/mav_planning_rviz/src/planning_panel.cpp index 4de3ed54..4498ed73 100644 --- a/mav_planning_rviz/src/planning_panel.cpp +++ b/mav_planning_rviz/src/planning_panel.cpp @@ -511,7 +511,6 @@ void PlanningPanel::setPlannerModeServiceCoverage() { callSetPlannerStateService("/terrain_planner/set_planner_state", 6); } - void PlanningPanel::callSetPlannerStateService(std::string service_name, const int mode) { std::thread t([service_name, mode] { planner_msgs::SetPlannerState req; @@ -630,8 +629,8 @@ void PlanningPanel::plannerstateCallback(const planner_msgs::NavigationStatus& m set_planner_state_buttons_[0]->setDisabled(true); // NAVIGATE set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT set_planner_state_buttons_[2]->setDisabled(false); // ABORT - set_planner_state_buttons_[3]->setDisabled(true); // RETURN - set_planner_state_buttons_[4]->setDisabled(true); // Coverage + set_planner_state_buttons_[3]->setDisabled(true); // RETURN + set_planner_state_buttons_[4]->setDisabled(true); // Coverage break; } case PLANNER_STATE::ROLLOUT: { @@ -639,7 +638,7 @@ void PlanningPanel::plannerstateCallback(const planner_msgs::NavigationStatus& m set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT set_planner_state_buttons_[2]->setDisabled(false); // ABORT set_planner_state_buttons_[3]->setDisabled(true); // RETURN - set_planner_state_buttons_[4]->setDisabled(true); // Coverage + set_planner_state_buttons_[4]->setDisabled(true); // Coverage break; } case PLANNER_STATE::ABORT: { @@ -655,15 +654,15 @@ void PlanningPanel::plannerstateCallback(const planner_msgs::NavigationStatus& m set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT set_planner_state_buttons_[2]->setDisabled(false); // ABORT set_planner_state_buttons_[3]->setDisabled(true); // RETURN - set_planner_state_buttons_[4]->setDisabled(true); // Coverage + set_planner_state_buttons_[4]->setDisabled(true); // Coverage break; } case PLANNER_STATE::COVERAGE: { set_planner_state_buttons_[0]->setDisabled(true); // NAVIGATE set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT set_planner_state_buttons_[2]->setDisabled(false); // ABORT - set_planner_state_buttons_[3]->setDisabled(true); // RETURN - set_planner_state_buttons_[4]->setDisabled(true); // Coverage + set_planner_state_buttons_[3]->setDisabled(true); // RETURN + set_planner_state_buttons_[4]->setDisabled(true); // Coverage break; } } diff --git a/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h b/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h index 19505a33..14c31f07 100644 --- a/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h +++ b/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h @@ -85,6 +85,16 @@ class TerrainOmplRrt { */ void setupProblem(const Eigen::Vector3d& start_pos, const Eigen::Vector3d& goal, double start_loiter_radius); + /** + * @brief Setup problem with center position of start and goal loiter circle with specific radius + * + * @param start_pos + * @param goal + * @param start_loiter_radius + */ + void setupProblem(const Eigen::Vector3d& start_pos, const double start_loiter_radius, const Eigen::Vector3d& goal, + const Eigen::Vector3d& goal_vel); + /** * @brief Setup problem with position, velocity of the start and center of the goal loiter circle * diff --git a/terrain_planner/src/terrain_ompl_rrt.cpp b/terrain_planner/src/terrain_ompl_rrt.cpp index bca1bd3a..e8fdee86 100644 --- a/terrain_planner/src/terrain_ompl_rrt.cpp +++ b/terrain_planner/src/terrain_ompl_rrt.cpp @@ -162,6 +162,42 @@ void TerrainOmplRrt::setupProblem(const Eigen::Vector3d& start_pos, const Eigen: problem_setup_->setup(); } +void TerrainOmplRrt::setupProblem(const Eigen::Vector3d& start_pos, const double start_loiter_radius, + const Eigen::Vector3d& goal, const Eigen::Vector3d& goal_vel) { + configureProblem(); + double radius = + problem_setup_->getStateSpace()->as()->getMinTurningRadius(); + double delta_theta = 0.1; + for (double theta = -M_PI; theta < M_PI; theta += (delta_theta * 2 * M_PI)) { + ompl::base::ScopedState start_ompl( + problem_setup_->getSpaceInformation()); + + start_ompl->setX(start_pos(0) + std::abs(start_loiter_radius) * std::cos(theta)); + start_ompl->setY(start_pos(1) + std::abs(start_loiter_radius) * std::sin(theta)); + start_ompl->setZ(start_pos(2)); + double start_yaw = bool(start_loiter_radius > 0) ? theta - M_PI_2 : theta + M_PI_2; + wrap_pi(start_yaw); + start_ompl->setYaw(start_yaw); + problem_setup_->addStartState(start_ompl); + } + + goal_states_ = std::make_shared(problem_setup_->getSpaceInformation()); + ompl::base::ScopedState goal_ompl( + problem_setup_->getSpaceInformation()); + goal_ompl->setX(goal(0)); + goal_ompl->setY(goal(1)); + goal_ompl->setZ(goal(2)); + double goal_yaw = std::atan2(goal_vel(1), goal_vel(0)); + goal_ompl->setYaw(goal_yaw); + goal_states_->addState(goal_ompl); + problem_setup_->setGoal(goal_states_); + + problem_setup_->setup(); + + auto planner_ptr = problem_setup_->getPlanner(); + // std::cout << "Planner Range: " << planner_ptr->as()->getRange() << std::endl; +} + void TerrainOmplRrt::setupProblem(const Eigen::Vector3d& start_pos, const Eigen::Vector3d& start_vel, const std::vector& goal_positions) { if (goal_positions.empty()) { From 284022005b5170fd69cb18806872d120152d837f Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Wed, 17 Apr 2024 18:30:43 +0200 Subject: [PATCH 04/10] Remove min altitude --- .../include/mav_planning_rviz/planning_panel.h | 2 -- mav_planning_rviz/src/planning_panel.cpp | 7 +------ 2 files changed, 1 insertion(+), 8 deletions(-) diff --git a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h index d4b370ea..8a2a7359 100644 --- a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h +++ b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h @@ -71,7 +71,6 @@ class PlanningPanel : public rviz::Panel { void publishToController(); void terrainAlignmentStateChanged(int state); void EnableMaxAltitude(); - void DisableMaxAltitude(); void setPlannerModeServiceNavigate(); void setPlannerModeServiceRollout(); void setPlannerModeServiceAbort(); @@ -117,7 +116,6 @@ class PlanningPanel : public rviz::Panel { QPushButton* waypoint_button_; QPushButton* max_altitude_button_enable_; std::vector set_planner_state_buttons_; - QPushButton* max_altitude_button_disable_; QPushButton* controller_button_; QPushButton* load_terrain_button_; diff --git a/mav_planning_rviz/src/planning_panel.cpp b/mav_planning_rviz/src/planning_panel.cpp index 4498ed73..870c275b 100644 --- a/mav_planning_rviz/src/planning_panel.cpp +++ b/mav_planning_rviz/src/planning_panel.cpp @@ -103,8 +103,7 @@ QGroupBox* PlanningPanel::createPlannerCommandGroup() { trigger_planning_button_ = new QPushButton("Plan"); update_path_button_ = new QPushButton("Update Path"); planning_budget_editor_ = new QLineEdit; - max_altitude_button_enable_ = new QPushButton("Enable Max altitude"); - max_altitude_button_disable_ = new QPushButton("Disable Max altitude"); + max_altitude_button_enable_ = new QPushButton("Load Mission"); waypoint_button_ = new QPushButton("Disengage Planner"); controller_button_ = new QPushButton("Send To Controller"); @@ -121,7 +120,6 @@ QGroupBox* PlanningPanel::createPlannerCommandGroup() { service_layout->addWidget(trigger_planning_button_, 2, 2, 1, 2); service_layout->addWidget(new QLabel("Max Altitude Constraints:"), 3, 0, 1, 1); service_layout->addWidget(max_altitude_button_enable_, 3, 1, 1, 1); - service_layout->addWidget(max_altitude_button_disable_, 3, 2, 1, 1); service_layout->addWidget(planner_service_button_, 4, 0, 1, 2); service_layout->addWidget(waypoint_button_, 4, 2, 1, 2); @@ -138,7 +136,6 @@ QGroupBox* PlanningPanel::createPlannerCommandGroup() { connect(planning_budget_editor_, SIGNAL(editingFinished()), this, SLOT(updatePlanningBudget())); connect(trigger_planning_button_, SIGNAL(released()), this, SLOT(setPlanningBudgetService())); connect(max_altitude_button_enable_, SIGNAL(released()), this, SLOT(EnableMaxAltitude())); - connect(max_altitude_button_disable_, SIGNAL(released()), this, SLOT(DisableMaxAltitude())); connect(controller_button_, SIGNAL(released()), this, SLOT(publishToController())); connect(terrain_align_checkbox_, SIGNAL(stateChanged(int)), this, SLOT(terrainAlignmentStateChanged(int))); @@ -392,8 +389,6 @@ void PlanningPanel::publishWaypoint() { void PlanningPanel::EnableMaxAltitude() { setMaxAltitudeConstrant(true); } -void PlanningPanel::DisableMaxAltitude() { setMaxAltitudeConstrant(false); } - void PlanningPanel::setMaxAltitudeConstrant(bool set_constraint) { std::cout << "[PlanningPanel] Loading new terrain:" << planner_name_.toStdString() << std::endl; // Load new environment using a service From caa4aa5b6eb9569cba3f196c38b6d2a5de36b22f Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Wed, 17 Apr 2024 19:17:35 +0200 Subject: [PATCH 05/10] Add mission loading button --- .../mav_planning_rviz/planning_panel.h | 2 +- mav_planning_rviz/src/planning_panel.cpp | 22 +++++++++++++++---- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h index 8a2a7359..527029ef 100644 --- a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h +++ b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h @@ -70,7 +70,7 @@ class PlanningPanel : public rviz::Panel { void publishWaypoint(); void publishToController(); void terrainAlignmentStateChanged(int state); - void EnableMaxAltitude(); + void loadMissionService(); void setPlannerModeServiceNavigate(); void setPlannerModeServiceRollout(); void setPlannerModeServiceAbort(); diff --git a/mav_planning_rviz/src/planning_panel.cpp b/mav_planning_rviz/src/planning_panel.cpp index 870c275b..bccee962 100644 --- a/mav_planning_rviz/src/planning_panel.cpp +++ b/mav_planning_rviz/src/planning_panel.cpp @@ -118,7 +118,7 @@ QGroupBox* PlanningPanel::createPlannerCommandGroup() { service_layout->addWidget(new QLabel("Planning budget:"), 2, 0, 1, 1); service_layout->addWidget(planning_budget_editor_, 2, 1, 1, 1); service_layout->addWidget(trigger_planning_button_, 2, 2, 1, 2); - service_layout->addWidget(new QLabel("Max Altitude Constraints:"), 3, 0, 1, 1); + service_layout->addWidget(new QLabel("Load Preplanned Mission:"), 3, 0, 1, 1); service_layout->addWidget(max_altitude_button_enable_, 3, 1, 1, 1); service_layout->addWidget(planner_service_button_, 4, 0, 1, 2); service_layout->addWidget(waypoint_button_, 4, 2, 1, 2); @@ -135,7 +135,7 @@ QGroupBox* PlanningPanel::createPlannerCommandGroup() { connect(waypoint_button_, SIGNAL(released()), this, SLOT(publishWaypoint())); connect(planning_budget_editor_, SIGNAL(editingFinished()), this, SLOT(updatePlanningBudget())); connect(trigger_planning_button_, SIGNAL(released()), this, SLOT(setPlanningBudgetService())); - connect(max_altitude_button_enable_, SIGNAL(released()), this, SLOT(EnableMaxAltitude())); + connect(max_altitude_button_enable_, SIGNAL(released()), this, SLOT(loadMissionService())); connect(controller_button_, SIGNAL(released()), this, SLOT(publishToController())); connect(terrain_align_checkbox_, SIGNAL(stateChanged(int)), this, SLOT(terrainAlignmentStateChanged(int))); @@ -387,8 +387,6 @@ void PlanningPanel::publishWaypoint() { t.detach(); } -void PlanningPanel::EnableMaxAltitude() { setMaxAltitudeConstrant(true); } - void PlanningPanel::setMaxAltitudeConstrant(bool set_constraint) { std::cout << "[PlanningPanel] Loading new terrain:" << planner_name_.toStdString() << std::endl; // Load new environment using a service @@ -486,6 +484,22 @@ void PlanningPanel::setPlanningBudgetService() { t.detach(); } +void PlanningPanel::loadMissionService() { + std::string service_name = "/terrain_planner/load_mission"; + std::thread t([service_name] { + planner_msgs::SetVector3 req; + try { + ROS_DEBUG_STREAM("Service name: " << service_name); + if (!ros::service::call(service_name, req)) { + std::cout << "Couldn't call service: " << service_name << std::endl; + } + } catch (const std::exception& e) { + std::cout << "Service Exception: " << e.what() << std::endl; + } + }); + t.detach(); +} + void PlanningPanel::setPlannerModeServiceNavigate() { callSetPlannerStateService("/terrain_planner/set_planner_state", 2); } From bb86b6dc25bc371b831a1222bf10a8d340d3f49b Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Wed, 17 Apr 2024 19:58:27 +0200 Subject: [PATCH 06/10] Add trigger field --- terrain_navigation/include/terrain_navigation/path_segment.h | 1 + 1 file changed, 1 insertion(+) diff --git a/terrain_navigation/include/terrain_navigation/path_segment.h b/terrain_navigation/include/terrain_navigation/path_segment.h index 73b9c9cc..f67a7bfe 100644 --- a/terrain_navigation/include/terrain_navigation/path_segment.h +++ b/terrain_navigation/include/terrain_navigation/path_segment.h @@ -268,6 +268,7 @@ class PathSegment { bool viewed{false}; bool reached{false}; bool is_periodic{false}; + bool trigger{false}; private: }; From d7f8b752146288fc10d94f09268755601433bdf3 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Wed, 17 Apr 2024 22:12:25 +0200 Subject: [PATCH 07/10] Use next segment idx --- terrain_navigation/include/terrain_navigation/path.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/terrain_navigation/include/terrain_navigation/path.h b/terrain_navigation/include/terrain_navigation/path.h index 5df01499..e2b7887d 100644 --- a/terrain_navigation/include/terrain_navigation/path.h +++ b/terrain_navigation/include/terrain_navigation/path.h @@ -125,18 +125,18 @@ class Path { Eigen::Vector3d closest_point; Eigen::Vector3d tangent; double curvature; - int segment_idx{-1}; + int next_segment_idx{0}; for (auto &segment : segments) { - segment_idx++; + next_segment_idx++; if (segment.reached && (&segment != &segments.back())) continue; auto theta = segment.getClosestPoint(position, closest_point, tangent, curvature); // If current segment is a full circle, and has a next segment, escape when close to start of next segment if (segment.is_periodic && (&segment != &segments.back())) { // Segment is a terminal periodic set - Eigen::Vector3d next_segment_start = segments[segment_idx].states.front().position; + Eigen::Vector3d next_segment_start = segments[next_segment_idx].states.front().position; if ((closest_point - next_segment_start).norm() < epsilon_) { segment.reached = true; - return segments[segment_idx]; + return segments[next_segment_idx]; // Return next segment } } From 67be941fa84b5ac18c89d758ebfe5d819b3f1329 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Fri, 19 Apr 2024 17:54:57 +0200 Subject: [PATCH 08/10] Print segment index Remove printout --- terrain_navigation/include/terrain_navigation/path.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/terrain_navigation/include/terrain_navigation/path.h b/terrain_navigation/include/terrain_navigation/path.h index e2b7887d..d46b8480 100644 --- a/terrain_navigation/include/terrain_navigation/path.h +++ b/terrain_navigation/include/terrain_navigation/path.h @@ -125,18 +125,18 @@ class Path { Eigen::Vector3d closest_point; Eigen::Vector3d tangent; double curvature; - int next_segment_idx{0}; + int segment_idx{-1}; for (auto &segment : segments) { - next_segment_idx++; + segment_idx++; if (segment.reached && (&segment != &segments.back())) continue; auto theta = segment.getClosestPoint(position, closest_point, tangent, curvature); // If current segment is a full circle, and has a next segment, escape when close to start of next segment if (segment.is_periodic && (&segment != &segments.back())) { // Segment is a terminal periodic set - Eigen::Vector3d next_segment_start = segments[next_segment_idx].states.front().position; + Eigen::Vector3d next_segment_start = segments[segment_idx+1].states.front().position; if ((closest_point - next_segment_start).norm() < epsilon_) { segment.reached = true; - return segments[next_segment_idx]; // Return next segment + return segments[segment_idx+1]; // Return next segment } } From 9f0e382cfe64b5260818ad1bd8326ed43f739e54 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Wed, 22 May 2024 08:59:13 +0200 Subject: [PATCH 09/10] Fix problems with floating point conversions --- terrain_navigation/src/data_logger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/terrain_navigation/src/data_logger.cpp b/terrain_navigation/src/data_logger.cpp index 5b55cf55..8ee66c33 100644 --- a/terrain_navigation/src/data_logger.cpp +++ b/terrain_navigation/src/data_logger.cpp @@ -64,7 +64,7 @@ void DataLogger::writeToFile(const std::string path) { if (std::string* s = std::any_cast(&data.at(key))) { output_file << std::any_cast(data.at(key)) << field_seperator; } else if (double* i = std::any_cast(&data.at(key))) { - output_file << std::any_cast(data.at(key)) << field_seperator; + output_file << std::to_string(std::any_cast(data.at(key))) << field_seperator; } } output_file << "\n"; From 6e30f0df2c6e6b8bca44e193b599a119a7dd6ad8 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Sun, 21 Jul 2024 14:00:45 +0200 Subject: [PATCH 10/10] Add segment end --- planner_msgs/msg/PathSegment.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/planner_msgs/msg/PathSegment.msg b/planner_msgs/msg/PathSegment.msg index 86616495..eeff3264 100644 --- a/planner_msgs/msg/PathSegment.msg +++ b/planner_msgs/msg/PathSegment.msg @@ -7,6 +7,7 @@ bool reached bool periodic std_msgs/Float64 segment_length geometry_msgs/Vector3 segment_start +geometry_msgs/Vector3 segment_end geometry_msgs/Vector3 segment_tangent std_msgs/Float64 curvature