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..527029ef 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; @@ -70,12 +70,12 @@ class PlanningPanel : public rviz::Panel { void publishWaypoint(); void publishToController(); void terrainAlignmentStateChanged(int state); - void EnableMaxAltitude(); - void DisableMaxAltitude(); + void loadMissionService(); void setPlannerModeServiceNavigate(); void setPlannerModeServiceRollout(); void setPlannerModeServiceAbort(); void setPlannerModeServiceReturn(); + void setPlannerModeServiceCoverage(); protected: // Set up the layout, only called by the constructor. @@ -116,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 89f91928..bccee962 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; } @@ -100,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"); @@ -116,9 +118,8 @@ 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(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); @@ -134,8 +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_disable_, SIGNAL(released()), this, SLOT(DisableMaxAltitude())); + 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,10 +387,6 @@ void PlanningPanel::publishWaypoint() { t.detach(); } -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 @@ -488,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); } @@ -504,6 +516,10 @@ 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,13 +631,15 @@ 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: { 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_[3]->setDisabled(true); // RETURN + set_planner_state_buttons_[4]->setDisabled(true); // Coverage break; } case PLANNER_STATE::ROLLOUT: { @@ -629,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(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 +655,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 +663,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; } } 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 diff --git a/terrain_navigation/include/terrain_navigation/path.h b/terrain_navigation/include/terrain_navigation/path.h index 5df01499..d46b8480 100644 --- a/terrain_navigation/include/terrain_navigation/path.h +++ b/terrain_navigation/include/terrain_navigation/path.h @@ -133,10 +133,10 @@ class Path { // 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[segment_idx+1].states.front().position; if ((closest_point - next_segment_start).norm() < epsilon_) { segment.reached = true; - return segments[segment_idx]; + return segments[segment_idx+1]; // Return next segment } } 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: }; 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"; 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()) {