Backport changes

Author: Jaeyoung-Lim

terrain_navigation_ros/CMakeLists.txt

@@ -7,6 +7,8 @@ find_package(catkin REQUIRED dynamic_reconfigure)
 generate_dynamic_reconfigure_options(
   cfg/HeightRateTuning.cfg
 )
+find_package(Boost REQUIRED COMPONENTS serialization system filesystem)
+
 
 find_package(catkin REQUIRED COMPONENTS
   roscpp
@@ -41,8 +43,8 @@ include_directories(
 add_library(${PROJECT_NAME}
   src/terrain_planner.cpp
 )
-add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${GeographicLib_LIBRARIES} ${OMPL_LIBRARIES})
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg ${Boost_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${GeographicLib_LIBRARIES} ${OMPL_LIBRARIES} ${Boost_LIBRARIES})
 
 add_executable(terrain_planner_node
   src/terrain_planner_node.cpp

terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h

@@ -43,7 +43,6 @@
 
 #include <ros/callback_queue.h>
 #include <ros/ros.h>
-#include "terrain_planner/terrain_ompl_rrt.h"
 
 #include <geographic_msgs/GeoPointStamped.h>
 #include <geometry_msgs/PoseStamped.h>
@@ -52,6 +51,7 @@
 #include <mavros_msgs/State.h>
 #include <mavros_msgs/WaypointList.h>
 #include <nav_msgs/Path.h>
+#include <sensor_msgs/NavSatFix.h>
 #include <visualization_msgs/Marker.h>
 
 #include <Eigen/Dense>
@@ -61,10 +61,10 @@
 
 #include "terrain_planner/common.h"
 #include "terrain_planner/maneuver_library.h"
-// #include "terrain_planner/mcts_planner.h"
-#include "terrain_planner/primitive_planner.h"
+#include "terrain_planner/terrain_ompl_rrt.h"
 #include "terrain_planner/visualization.h"
 
+#include <planner_msgs/SetPlannerState.h>
 #include <planner_msgs/SetService.h>
 #include <planner_msgs/SetString.h>
 #include <planner_msgs/SetVector3.h>
@@ -73,11 +73,11 @@
 #include <GeographicLib/LocalCartesian.hpp>
 
 #include <dynamic_reconfigure/server.h>
-#include "terrain_navigation_ros/HeightRateTuningConfig.h"
+// #include "terrain_navigation_ros/HeightRateTuningConfig.h"
 
-enum class SETPOINT_MODE { STATE, PATH };
+enum class PLANNER_MODE { ACTIVE_MAPPING, EMERGENCY_ABORT, EXHAUSTIVE, GLOBAL, GLOBAL_REPLANNING, RANDOM, RETURN };
 
-enum class PLANNER_MODE { EXHAUSTIVE, MCTS, GLOBAL, GLOBAL_REPLANNING, RANDOM };
+enum class PLANNER_STATE { HOLD = 1, NAVIGATE = 2, ROLLOUT = 3, ABORT = 4, RETURN = 5 };
 
 class TerrainPlanner {
  public:
@@ -88,9 +88,11 @@ class TerrainPlanner {
  private:
   void cmdloopCallback(const ros::TimerEvent &event);
   void statusloopCallback(const ros::TimerEvent &event);
+  void plannerloopCallback(const ros::TimerEvent &event);
   void publishTrajectory(std::vector<Eigen::Vector3d> trajectory);
   // States from vehicle
-  void mavposeCallback(const geometry_msgs::PoseStamped &msg);
+  void mavLocalPoseCallback(const geometry_msgs::PoseStamped &msg);
+  void mavGlobalPoseCallback(const sensor_msgs::NavSatFix &msg);
   void mavtwistCallback(const geometry_msgs::TwistStamped &msg);
   void mavstateCallback(const mavros_msgs::State::ConstPtr &msg);
   void mavGlobalOriginCallback(const geographic_msgs::GeoPointStampedConstPtr &msg);
@@ -103,9 +105,11 @@ class TerrainPlanner {
   bool setCurrentSegmentCallback(planner_msgs::SetService::Request &req, planner_msgs::SetService::Response &res);
   bool setStartLoiterCallback(planner_msgs::SetService::Request &req, planner_msgs::SetService::Response &res);
   bool setPlanningCallback(planner_msgs::SetVector3::Request &req, planner_msgs::SetVector3::Response &res);
+  bool setPlannerStateCallback(planner_msgs::SetPlannerState::Request &req,
+                               planner_msgs::SetPlannerState::Response &res);
   bool setPathCallback(planner_msgs::SetVector3::Request &req, planner_msgs::SetVector3::Response &res);
 
-  void MapPublishOnce();
+  void MapPublishOnce(const ros::Publisher &pub, const grid_map::GridMap &map);
   void publishPositionHistory(ros::Publisher &pub, const Eigen::Vector3d &position,
                               std::vector<geometry_msgs::PoseStamped> &history_vector);
   void publishPositionSetpoints(const ros::Publisher &pub, const Eigen::Vector3d &position,
@@ -117,20 +121,45 @@ class TerrainPlanner {
   void publishVelocityMarker(const ros::Publisher &pub, const Eigen::Vector3d &position,
                              const Eigen::Vector3d &velocity);
   void publishPathSetpoints(const Eigen::Vector3d &position, const Eigen::Vector3d &velocity);
-  void publishVehiclePose(const Eigen::Vector3d &position, const Eigen::Vector4d &attitude);
-  void publishViewpoints(std::vector<ViewPoint> &viewpoint_vector);
   void publishPathSegments(ros::Publisher &pub, Path &trajectory);
   void publishGoal(const ros::Publisher &pub, const Eigen::Vector3d &position, const double radius,
                    Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0), std::string name_space = "goal");
+  void publishRallyPoints(const ros::Publisher &pub, const std::vector<Eigen::Vector3d> &positions, const double radius,
+                          Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0),
+                          std::string name_space = "rallypoints");
+  visualization_msgs::Marker getGoalMarker(const int id, const Eigen::Vector3d &position, const double radius,
+                                           const Eigen::Vector3d color);
   void generateCircle(const Eigen::Vector3d end_position, const Eigen::Vector3d end_velocity,
                       const Eigen::Vector3d center_pos, PathSegment &trajectory);
-  void dynamicReconfigureCallback(terrain_navigation_ros::HeightRateTuningConfig &config, uint32_t level);
+  // void dynamicReconfigureCallback(terrain_navigation_ros::HeightRateTuningConfig &config, uint32_t level);
+
+  void printPlannerState(PLANNER_STATE state) {
+    switch (state) {
+      case PLANNER_STATE::HOLD:  // Fallthrough
+        std::cout << "PLANNER_STATE::HOLD" << std::endl;
+        break;
+      case PLANNER_STATE::NAVIGATE:  // Fallthrough
+        std::cout << "PLANNER_STATE::NAVIGATE" << std::endl;
+        break;
+      case PLANNER_STATE::ROLLOUT:
+        std::cout << "PLANNER_STATE::ROLLOUT" << std::endl;
+        break;
+      case PLANNER_STATE::ABORT:
+        std::cout << "PLANNER_STATE::ABORT" << std::endl;
+        break;
+    }
+  }
+
+  Eigen::Vector4d rpy2quaternion(double roll, double pitch, double yaw);
+
+  PLANNER_STATE finiteStateMachine(const PLANNER_STATE current_state, const PLANNER_STATE query_state);
 
   ros::NodeHandle nh_;
   ros::NodeHandle nh_private_;
   ros::Publisher vehicle_path_pub_;
   ros::Publisher grid_map_pub_;
   ros::Publisher vehicle_pose_pub_;
+  ros::Publisher camera_pose_pub_;
   ros::Publisher posehistory_pub_;
   ros::Publisher referencehistory_pub_;
   ros::Publisher position_setpoint_pub_;
@@ -139,13 +168,16 @@ class TerrainPlanner {
   ros::Publisher path_target_pub_;
   ros::Publisher planner_status_pub_;
   ros::Publisher goal_pub_;
+  ros::Publisher rallypoint_pub_;
   ros::Publisher candidate_goal_pub_;
   ros::Publisher candidate_start_pub_;
   ros::Publisher viewpoint_pub_;
+  ros::Publisher planned_viewpoint_pub_;
   ros::Publisher tree_pub_;
   ros::Publisher vehicle_velocity_pub_;
   ros::Publisher path_segment_pub_;
-  ros::Subscriber mavpose_sub_;
+  ros::Subscriber mavlocalpose_sub_;
+  ros::Subscriber mavglobalpose_sub_;
   ros::Subscriber mavtwist_sub_;
   ros::Subscriber mavstate_sub_;
   ros::Subscriber mavmission_sub_;
@@ -160,42 +192,49 @@ class TerrainPlanner {
   ros::ServiceServer setplanning_serviceserver_;
   ros::ServiceServer updatepath_serviceserver_;
   ros::ServiceServer setcurrentsegment_serviceserver_;
+  ros::ServiceServer setplannerstate_service_server_;
   ros::ServiceClient msginterval_serviceclient_;
 
-  ros::Timer cmdloop_timer_, statusloop_timer_;
+  ros::Timer cmdloop_timer_, statusloop_timer_, plannerloop_timer_;
   ros::Time plan_time_;
   ros::Time last_triggered_time_;
   Eigen::Vector3d goal_pos_{Eigen::Vector3d(0.0, 0.0, 20.0)};
   Eigen::Vector3d start_pos_{Eigen::Vector3d(0.0, 0.0, 20.0)};
+  Eigen::Vector3d home_position_{Eigen::Vector3d(0.0, 0.0, 20.0)};
+  double home_position_radius_{0.0};
   Eigen::Vector3d tracking_error_{Eigen::Vector3d::Zero()};
+  ros::CallbackQueue plannerloop_queue_;
   ros::CallbackQueue statusloop_queue_;
   ros::CallbackQueue cmdloop_queue_;
+  std::unique_ptr<ros::AsyncSpinner> plannerloop_spinner_;
   std::unique_ptr<ros::AsyncSpinner> statusloop_spinner_;
   std::unique_ptr<ros::AsyncSpinner> cmdloop_spinner_;
 
-  dynamic_reconfigure::Server<terrain_navigation_ros::HeightRateTuningConfig> server;
-  dynamic_reconfigure::Server<terrain_navigation_ros::HeightRateTuningConfig>::CallbackType f;
+  // dynamic_reconfigure::Server<terrain_navigation_ros::HeightRateTuningConfig> server;
+  // dynamic_reconfigure::Server<terrain_navigation_ros::HeightRateTuningConfig>::CallbackType f;
 
-  SETPOINT_MODE setpoint_mode_{SETPOINT_MODE::STATE};
   PLANNER_MODE planner_mode_{PLANNER_MODE::EXHAUSTIVE};
+  PLANNER_STATE planner_state_{PLANNER_STATE::HOLD};
+  PLANNER_STATE query_planner_state_{PLANNER_STATE::HOLD};
 
   std::shared_ptr<ManeuverLibrary> maneuver_library_;
-  // std::shared_ptr<MctsPlanner> mcts_planner_;
-  std::shared_ptr<PrimitivePlanner> primitive_planner_;
   std::shared_ptr<TerrainMap> terrain_map_;
+
   std::shared_ptr<fw_planning::spaces::DubinsAirplaneStateSpace> dubins_state_space_;
   // std::shared_ptr<ViewUtilityMap> viewutility_map_;
   std::shared_ptr<TerrainOmplRrt> global_planner_;
   std::shared_ptr<Profiler> planner_profiler_;
   Path reference_primitive_;
   Path candidate_primitive_;
+  Path rollout_primitive_;
   mavros_msgs::State current_state_;
   std::optional<GeographicLib::LocalCartesian> enu_;
 
   std::mutex goal_mutex_;  // protects g_i
 
   std::vector<Eigen::Vector3d> vehicle_position_history_;
   std::vector<ViewPoint> added_viewpoint_list;
+  std::vector<ViewPoint> planned_viewpoint_list;
   std::vector<geometry_msgs::PoseStamped> posehistory_vector_;
   std::vector<geometry_msgs::PoseStamped> referencehistory_vector_;
   std::vector<ViewPoint> viewpoints_;
@@ -204,11 +243,14 @@ class TerrainPlanner {
   Eigen::Vector4d vehicle_attitude_{Eigen::Vector4d(1.0, 0.0, 0.0, 0.0)};
   Eigen::Vector3d last_planning_position_{Eigen::Vector3d::Zero()};
   Eigen::Vector3d previous_start_position_{Eigen::Vector3d::Zero()};
+  Eigen::Vector3d previous_return_start_position_{Eigen::Vector3d::Zero()};
   Eigen::Vector3d mission_loiter_center_{Eigen::Vector3d::Zero()};
 
+  std::vector<Eigen::Vector3d> rally_points;
+
   // Altitude tracking loop parameters
   /// TODO: This needs to be handed over to TECS
-  double K_z_{0.5};
+  double K_z_{2.0};
   double cruise_speed_{20.0};
   double max_climb_rate_control_{5.0};
 
@@ -229,6 +271,8 @@ class TerrainPlanner {
   bool map_initialized_{false};
   bool planner_enabled_{false};
   bool problem_updated_{true};
+  bool found_solution_{false};
+  bool found_return_solution_{false};
 };
 
 #endif

terrain_navigation_ros/include/terrain_navigation_ros/visualization.h

@@ -0,0 +1,143 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 Jaeyoung Lim. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name terrain-navigation nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef TERRAIN_NAVIGATION_ROS_VISUALIZATION_H
+#define TERRAIN_NAVIGATION_ROS_VISUALIZATION_H
+
+#include <ros/ros.h>
+#include <Eigen/Dense>
+
+#include <geometry_msgs/Pose.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+inline geometry_msgs::Point toPoint(const Eigen::Vector3d &p) {
+  geometry_msgs::Point position;
+  position.x = p(0);
+  position.y = p(1);
+  position.z = p(2);
+  return position;
+}
+
+inline void publishVehiclePose(const ros::Publisher pub, const Eigen::Vector3d &position,
+                               const Eigen::Vector4d &attitude, std::string mesh_resource_path) {
+  Eigen::Vector4d mesh_attitude =
+      quatMultiplication(attitude, Eigen::Vector4d(std::cos(M_PI / 2), 0.0, 0.0, std::sin(M_PI / 2)));
+  geometry_msgs::Pose vehicle_pose = vector3d2PoseMsg(position, mesh_attitude);
+  visualization_msgs::Marker marker;
+  marker.header.stamp = ros::Time::now();
+  marker.header.frame_id = "map";
+  marker.type = visualization_msgs::Marker::MESH_RESOURCE;
+  marker.ns = "my_namespace";
+  marker.mesh_resource = "package://terrain_planner/" + mesh_resource_path;
+  marker.scale.x = 10.0;
+  marker.scale.y = 10.0;
+  marker.scale.z = 10.0;
+  marker.color.a = 0.5;  // Don't forget to set the alpha!
+  marker.color.r = 0.5;
+  marker.color.g = 0.5;
+  marker.color.b = 0.5;
+  marker.pose = vehicle_pose;
+  pub.publish(marker);
+}
+
+inline visualization_msgs::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint,
+                                                      Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) {
+  double scale{15};  // Size of the viewpoint markers
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time();
+  marker.ns = "my_namespace";
+  marker.id = id;
+  marker.type = visualization_msgs::Marker::LINE_LIST;
+  marker.action = visualization_msgs::Marker::ADD;
+  const Eigen::Vector3d position = viewpoint.getCenterLocal();
+  std::vector<geometry_msgs::Point> points;
+  std::vector<Eigen::Vector3d> corner_ray_vectors = viewpoint.getCornerRayVectors();
+  std::vector<Eigen::Vector3d> vertex;
+  for (auto &corner_ray : corner_ray_vectors) {
+    vertex.push_back(position + scale * corner_ray);
+  }
+
+  for (size_t i = 0; i < vertex.size(); i++) {
+    points.push_back(toPoint(position));  // Viewpoint center
+    points.push_back(toPoint(vertex[i]));
+    points.push_back(toPoint(vertex[i]));
+    points.push_back(toPoint(vertex[(i + 1) % vertex.size()]));
+  }
+
+  marker.points = points;
+  marker.pose.orientation.x = 0.0;
+  marker.pose.orientation.y = 0.0;
+  marker.pose.orientation.z = 0.0;
+  marker.pose.orientation.w = 1.0;
+  marker.scale.x = 1.0;
+  marker.scale.y = 1.0;
+  marker.scale.z = 1.0;
+  marker.color.a = 1.0;  // Don't forget to set the alpha!
+  marker.color.r = color(0);
+  marker.color.g = color(1);
+  marker.color.b = color(2);
+  return marker;
+}
+
+inline void publishCameraView(const ros::Publisher pub, const Eigen::Vector3d &position,
+                              const Eigen::Vector4d &attitude) {
+  visualization_msgs::Marker marker;
+  ViewPoint viewpoint(-1, position, attitude);
+  marker = Viewpoint2MarkerMsg(viewpoint.getIndex(), viewpoint);
+  pub.publish(marker);
+}
+
+inline void publishViewpoints(const ros::Publisher pub, std::vector<ViewPoint> &viewpoint_vector,
+                              Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) {
+  visualization_msgs::MarkerArray msg;
+
+  std::vector<visualization_msgs::Marker> marker;
+  visualization_msgs::Marker mark;
+  mark.action = visualization_msgs::Marker::DELETEALL;
+  marker.push_back(mark);
+  msg.markers = marker;
+  pub.publish(msg);
+
+  std::vector<visualization_msgs::Marker> viewpoint_marker_vector;
+  int i = 0;
+  for (auto viewpoint : viewpoint_vector) {
+    viewpoint_marker_vector.insert(viewpoint_marker_vector.begin(), Viewpoint2MarkerMsg(i, viewpoint, color));
+    i++;
+  }
+  msg.markers = viewpoint_marker_vector;
+  pub.publish(msg);
+}
+
+#endif