MPPI pathtracking

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<QPushButton*> set_planner_state_buttons_;
-  QPushButton* max_altitude_button_disable_;
   QPushButton* controller_button_;
   QPushButton* load_terrain_button_;
 

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,34 +631,47 @@ 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: {
       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;
     }
     case PLANNER_STATE::ABORT: {
       set_planner_state_buttons_[0]->setDisabled(true);  // NAVIGATE
       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: {
       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;
+    }
+    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;
     }
   }

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
 

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
         }
       }
 

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:
 };

terrain_navigation/src/data_logger.cpp

@@ -64,7 +64,7 @@ void DataLogger::writeToFile(const std::string path) {
       if (std::string* s = std::any_cast<std::string>(&data.at(key))) {
         output_file << std::any_cast<std::string&>(data.at(key)) << field_seperator;
       } else if (double* i = std::any_cast<double>(&data.at(key))) {
-        output_file << std::any_cast<double&>(data.at(key)) << field_seperator;
+        output_file << std::to_string(std::any_cast<double&>(data.at(key))) << field_seperator;
       }
     }
     output_file << "\n";

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
    *

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<fw_planning::spaces::DubinsAirplaneStateSpace>()->getMinTurningRadius();
+  double delta_theta = 0.1;
+  for (double theta = -M_PI; theta < M_PI; theta += (delta_theta * 2 * M_PI)) {
+    ompl::base::ScopedState<fw_planning::spaces::DubinsAirplaneStateSpace> 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<ompl::base::GoalStates>(problem_setup_->getSpaceInformation());
+  ompl::base::ScopedState<fw_planning::spaces::DubinsAirplaneStateSpace> 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<ompl::geometric::RRTstar>()->getRange() << std::endl;
+}
+
 void TerrainOmplRrt::setupProblem(const Eigen::Vector3d& start_pos, const Eigen::Vector3d& start_vel,
                                   const std::vector<Eigen::Vector3d>& goal_positions) {
   if (goal_positions.empty()) {