Merge pull request #52 from rosflight/42-lat-long-input-to-path_planner

42 lat long input to path planner

README.md

@@ -66,21 +66,22 @@ Loading the mission can be done using
 where `FILENAME` is the absolute path to the mission .yaml file.
 Note that the origin (0,0,0) is placed at the GNSS location where ROSplane was initialized.
 
-> **Important**: All waypoints must include a valid `[X, Y, Z]` value and a valid `va_d` value.
+> **Important**: All waypoints must include a valid `[X, Y, Z]`, `va_d`, and `lla` values.
 
-Alternatively, you can add a waypoint one at a time by calling the appropriate service,
-```ros2 service call /add_waypoint rosplane_msgs/srv/AddWaypoint "{w: [X, Y, Z], chi_d: CHI_D, use_chi: USE_CHI, va_d: VA_D}"```
-where `[X, Y, Z]` is the NED position of the waypoint from the origin (in meters), `CHI_D` is the desired heading at the waypoint, and `VA_D` is the airspeed at the waypoint.
+Alternatively, you can add a waypoint one at a time by calling the appropriate service
+
+```ros2 service call /add_waypoint rosplane_msgs/srv/AddWaypoint "{w: [X, Y, Z], chi_d: CHI_D, lla: USE_LLA, use_chi: USE_CHI, va_d: VA_D}"```
+
+where `[X, Y, Z]` is the NED position of the waypoint from the origin (in meters) OR the GNSS location of the waypoint (LLA), `CHI_D` is the desired heading at the waypoint, and `VA_D` is the airspeed at the waypoint.
+Set the `lla` field to `true` if the waypoint `[X, Y, Z]` field is given in GNSS coordinates and `false` if given in NED coordinates.
 Corners in the path are controlled by `USE_CHI`, where a value of `True` will cause ROSplane to use a Dubins path planner and a value of `False` will cause a fillet path planner to be used.
-Adding waypoints can be done after loading from a file.
+Adding waypoints can be done at any time, even after loading from a file.
 
-Clearing waypoints can be done using
-```ros2 service call /clear_waypoints std_msgs/srv/Trigger```.
+Clearing waypoints can be done using `ros2 service call /clear_waypoints std_msgs/srv/Trigger`.
 
 ### Publishing Waypoints
 
 The `path_planner` node automatically publishes a small number of waypoints (default is 3) at the beginning of this mission.
 This number is controlled by the `num_waypoints_to_publish_at_start` ROS2 parameter. 
 
-Additional waypoints can be published using
-`ros2 service call /publish_next_waypoint std_srvs/srv/Trigger`.
+Additional waypoints can be published using `ros2 service call /publish_next_waypoint std_srvs/srv/Trigger`.

rosplane/params/fixedwing_mission.yaml

@@ -1,21 +1,25 @@
-# WAYPOINT 1
+# WAYPOINTS
 wp:
-  w: [0.0, 0.0, -50.0]
+  w: [40.247, -111.648, 1700.0]
   chi_d: 1.1518
+  lla: True
   use_chi: False
   va_d: 25.0
 wp:
   w: [1000.0, 400.0, -50.0]
   chi_d: 1.1518
+  lla: False
   use_chi: False
   va_d: 25.0
 wp:
   w: [1000.0, -500.0, -50.0]
   chi_d: 1.1518
+  lla: False
   use_chi: False
   va_d: 25.0
 wp:
   w: [0.0, -800.0, -50.0]
   chi_d: 1.1518
+  lla: False
   use_chi: False
   va_d: 25.0

rosplane/src/path_manager_example.cpp

@@ -38,7 +38,7 @@ void path_manager_example::manage(const input_s & input, output_s & output)
     if (float(std::chrono::system_clock::to_time_t(now) - std::chrono::system_clock::to_time_t(start_time)) >= 10.0) 
     { 
       // TODO: Add check to see if the aircraft has been armed. If not just send the warning once before flight then on the throttle after.
-      RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "No waypoits received, orbiting origin at " << default_altitude << " meters.");
+      RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "No waypoints received, orbiting origin at " << default_altitude << " meters.");
       output.flag = false; // Indicate that the path is an orbit.
       output.va_d = default_airspeed; // Set to the default_airspeed.
       output.c[0] = 0.0f; // Direcct the center of the orbit to the origin at the default default_altitude.

rosplane/src/path_planner.cpp

@@ -1,5 +1,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rosplane_msgs/msg/waypoint.hpp"
+#include "rosplane_msgs/msg/state.hpp"
 #include "rosplane_msgs/srv/add_waypoint.hpp"
 #include "rosflight_msgs/srv/param_file.hpp"
 #include <rclcpp/executors.hpp>
@@ -10,8 +11,9 @@
 #include <std_srvs/srv/trigger.hpp>
 #include <param_manager.hpp>
 #include <yaml-cpp/yaml.h>
+#include <cmath>
 
-#define NUM_WAYPOINTS_TO_PUBLISH_AT_START 3
+#define EARTH_RADIUS 6378145.0f
 
 using std::placeholders::_1;
 using std::placeholders::_2;
@@ -30,6 +32,7 @@ class path_planner : public rclcpp::Node
 
 private:
   rclcpp::Publisher<rosplane_msgs::msg::Waypoint>::SharedPtr waypoint_publisher;
+  rclcpp::Subscription<rosplane_msgs::msg::State>::SharedPtr state_subscription;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr next_waypoint_service;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr clear_waypoint_service;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr print_waypoint_service;
@@ -39,25 +42,20 @@ class path_planner : public rclcpp::Node
 
   bool publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req,
                              const std_srvs::srv::Trigger::Response::SharedPtr & res);
-
   bool update_path(const rosplane_msgs::srv::AddWaypoint::Request::SharedPtr & req,
                    const rosplane_msgs::srv::AddWaypoint::Response::SharedPtr & res);
-
   bool clear_path_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
-                  const std_srvs::srv::Trigger::Response::SharedPtr & res);
-
-  void clear_path();
-
+                  const std_srvs::srv::Trigger::Response::SharedPtr & res); void clear_path();
   bool print_path(const std_srvs::srv::Trigger::Request::SharedPtr & req,
                   const std_srvs::srv::Trigger::Response::SharedPtr & res);
-
   bool load_mission(const rosflight_msgs::srv::ParamFile::Request::SharedPtr & req,
                     const rosflight_msgs::srv::ParamFile::Response::SharedPtr & res);
-
   bool load_mission_from_file(const std::string& filename);
-
+  void state_callback(const rosplane_msgs::msg::State & msg);
   void waypoint_publish();
   void timer_callback();
+  std::array<double, 3> lla2ned(std::array<float, 3> lla);
+
   /**
    * This declares each parameter as a parameter so that the ROS2 parameter system can recognize each parameter.
    * It also sets the default parameter, which will then be overridden by a launch script.
@@ -69,6 +67,9 @@ class path_planner : public rclcpp::Node
   parametersCallback(const std::vector<rclcpp::Parameter> & parameters);
 
   int num_waypoints_published;
+  double initial_lat_;
+  double initial_lon_;
+  double initial_alt_;
 
   std::vector<rosplane_msgs::msg::Waypoint> wps;
 };
@@ -82,24 +83,22 @@ path_planner::path_planner()
   waypoint_publisher = this->create_publisher<rosplane_msgs::msg::Waypoint>("waypoint_path", qos_transient_local_10_);
 
   next_waypoint_service = this->create_service<std_srvs::srv::Trigger>(
-    "publish_next_waypoint",
-    std::bind(&path_planner::publish_next_waypoint, this, _1, _2));
+    "publish_next_waypoint", std::bind(&path_planner::publish_next_waypoint, this, _1, _2));
 
   add_waypoint_service = this->create_service<rosplane_msgs::srv::AddWaypoint>(
-    "add_waypoint",
-    std::bind(&path_planner::update_path, this, _1, _2));
+    "add_waypoint", std::bind(&path_planner::update_path, this, _1, _2));
 
   clear_waypoint_service = this->create_service<std_srvs::srv::Trigger>(
-    "clear_waypoints",
-    std::bind(&path_planner::clear_path_callback, this, _1, _2));
+    "clear_waypoints", std::bind(&path_planner::clear_path_callback, this, _1, _2));
 
   print_waypoint_service = this->create_service<std_srvs::srv::Trigger>(
-    "print_waypoints",
-    std::bind(&path_planner::print_path, this, _1, _2));
+    "print_waypoints", std::bind(&path_planner::print_path, this, _1, _2));
 
   load_mission_service = this->create_service<rosflight_msgs::srv::ParamFile>(
-    "load_mission_from_file",
-    std::bind(&path_planner::load_mission, this, _1, _2));
+    "load_mission_from_file", std::bind(&path_planner::load_mission, this, _1, _2));
+
+  state_subscription = this->create_subscription<rosplane_msgs::msg::State>("estimated_state", 10,
+    std::bind(&path_planner::state_callback, this, _1));
 
   // Set the parameter callback, for when parameters are changed.
   parameter_callback_handle_ = this->add_on_set_parameters_callback(
@@ -114,24 +113,38 @@ path_planner::path_planner()
   // Initialize by publishing a clear path command.
   // This makes sure rviz doesn't show stale waypoints if rosplane is restarted.
   clear_path();
+
+  // Print out helpful information
+  RCLCPP_INFO_STREAM(this->get_logger(), "Path Planner will publish the first {" << this->get_parameter("num_waypoints_to_publish_at_start").as_int() << "} available waypoints!");
 }
 
 path_planner::~path_planner() {}
 
 void path_planner::timer_callback() {
-  if (num_waypoints_published < NUM_WAYPOINTS_TO_PUBLISH_AT_START && num_waypoints_published < (int) wps.size()) {
+  int num_waypoints_to_publish_at_start = this->get_parameter("num_waypoints_to_publish_at_start").as_int();
+
+  if (num_waypoints_published < num_waypoints_to_publish_at_start && num_waypoints_published < (int) wps.size()) {
     waypoint_publish();
   }
 }
 
+void path_planner::state_callback(const rosplane_msgs::msg::State & msg) {
+  // Make sure initial LLA is not zero
+  if (fabs(msg.initial_lat) > 0.0 || fabs(msg.initial_lon) > 0.0 || fabs(msg.initial_alt) > 0.0) {
+    initial_lat_ = msg.initial_lat;
+    initial_lon_ = msg.initial_lon;
+    initial_alt_ = msg.initial_alt;
+  }
+}
+
 bool path_planner::publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req,
                                          const std_srvs::srv::Trigger::Response::SharedPtr & res)
 {
 
   if (num_waypoints_published < (int) wps.size()) {
     RCLCPP_INFO_STREAM(
       this->get_logger(),
-      "Publishing next waypoint, num_waypoints_published: " << num_waypoints_published);
+      "Publishing next waypoint, num_waypoints_published: " << num_waypoints_published + 1);
 
     waypoint_publish();
 
@@ -164,24 +177,37 @@ bool path_planner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::S
   rclcpp::Time now = this->get_clock()->now();
 
   new_waypoint.header.stamp = now;
-  new_waypoint.w = req->w;
+
+  // Convert to NED if given in LLA
+  std::string lla_or_ned = "LLA";
+  if (req->lla) {
+    std::array<double, 3> ned = lla2ned(req->w);
+
+    new_waypoint.w[0] = ned[0];
+    new_waypoint.w[1] = ned[1];
+    new_waypoint.w[2] = ned[2];
+  }
+  else {
+    new_waypoint.w = req->w;
+    lla_or_ned = "NED";
+  }
   new_waypoint.chi_d = req->chi_d;
+  new_waypoint.lla = req->lla;
   new_waypoint.use_chi = req->use_chi;
   new_waypoint.va_d = req->va_d;
   new_waypoint.set_current = req->set_current;
 
   if (req->publish_now) {
     wps.insert(wps.begin() + num_waypoints_published, new_waypoint);
     waypoint_publish();
-    res->message = "Adding waypoint was successful! Waypoint published.";
+    res->message = "Adding " + lla_or_ned + " waypoint was successful! Waypoint published.";
   }
   else {
     wps.push_back(new_waypoint);
-    res->message = "Adding waypoint was successful!";
+    res->message = "Adding " + lla_or_ned + " waypoint was successful!";
   }
 
   res->success = true;
-
   return true;
 }
 
@@ -213,7 +239,13 @@ bool path_planner::print_path(const std_srvs::srv::Trigger::Request::SharedPtr &
   for (int i=0; i < (int) wps.size(); ++i) {
     rosplane_msgs::msg::Waypoint wp = wps[i];
     output << std::endl << "----- WAYPOINT " << i << " -----" << std::endl;
-    output << "Position (NED, meters): [" << wp.w[0] << ", " << wp.w[1] << ", " << wp.w[2] << "]" << std::endl;
+
+    if (wp.lla) {
+      output << "Position (LLA): [" << wp.w[0] << ", " << wp.w[1] << ", " << wp.w[2] << "]" << std::endl;
+    }
+    else {
+      output << "Position (NED, meters): [" << wp.w[0] << ", " << wp.w[1] << ", " << wp.w[2] << "]" << std::endl;
+    }
     output << "Chi_d: " << wp.chi_d << " " << std::endl;
     output << "Va_d: " << wp.va_d << " " << std::endl;
     output << "use_chi: " << wp.use_chi;
@@ -245,6 +277,16 @@ bool path_planner::load_mission_from_file(const std::string& filename) {
 
       rosplane_msgs::msg::Waypoint new_wp;
       new_wp.w = wp["w"].as<std::array<float, 3>>();
+
+      // If LLA, convert to NED
+      if (wp["lla"].as<bool>()) {
+        std::array<double, 3> ned = lla2ned(new_wp.w);
+
+        new_wp.w[0] = ned[0];
+        new_wp.w[1] = ned[1];
+        new_wp.w[2] = ned[2];
+      }
+
       new_wp.chi_d = wp["chi_d"].as<double>();
       new_wp.use_chi = wp["use_chi"].as<bool>();
       new_wp.va_d = wp["va_d"].as<double>();
@@ -255,10 +297,31 @@ bool path_planner::load_mission_from_file(const std::string& filename) {
     return true;
   }
   catch (...) {
+    RCLCPP_ERROR_STREAM(this->get_logger(), "Error while parsing mission YAML file! Check inputs");
     return false;
   }
 }
 
+std::array<double, 3> path_planner::lla2ned(std::array<float, 3> lla) {
+  double lat1 = lla[0];
+  double lon1 = lla[1];
+  double alt1 = lla[2];
+
+  double lat0 = initial_lat_;
+  double lon0 = initial_lon_;
+  double alt0 = initial_alt_;
+
+  double n = EARTH_RADIUS * (lat1 - lat0) * M_PI / 180.0;
+  double e = EARTH_RADIUS * cos(lat0 * M_PI / 180.0) * (lon1 - lon0) * M_PI / 180.0;
+  double d = -(alt1 - alt0);
+
+  if (fabs(initial_lat_) == 0.0 || fabs(initial_lon_) == 0.0 || fabs(initial_alt_) == 0.0) {
+    RCLCPP_WARN_STREAM(this->get_logger(), "NED position set to [" << n << "," << e << "," << d << "]! Waypoints may be incorrect. Check GPS health");
+  }
+
+  return std::array<double, 3> {n, e, d};
+}
+
 rcl_interfaces::msg::SetParametersResult
 path_planner::parametersCallback(const std::vector<rclcpp::Parameter> & parameters)
 {

rosplane_gcs/src/rviz_waypoint_publisher.cpp

@@ -101,6 +101,8 @@ rviz_waypoint_publisher::~rviz_waypoint_publisher() {}
 void rviz_waypoint_publisher::new_wp_callback(const rosplane_msgs::msg::Waypoint & wp) {
     visualization_msgs::msg::Marker new_marker;
 
+    RCLCPP_INFO_STREAM(this->get_logger(), wp.lla);
+
     if (wp.clear_wp_list) {
         rclcpp::Time now = this->get_clock()->now();
         // Publish one for each ns

rosplane_msgs/msg/Waypoint.msg

@@ -4,7 +4,8 @@
 std_msgs/Header header
 
 # @warning w and Va_d always have to be valid; the chi_d is optional.
-float32[3] w		# Waypoint in local NED (m)
+float32[3] w		# Waypoint in local NED (m) or LLA
+bool lla            # Set this flag true if waypoint is LLA and not local NED
 float32 chi_d		# Desired course at this waypoint (rad)
 bool use_chi		# Use chi_d as specified, using a Dubin's path. Otherwise, use fillet
 float32 va_d		# Desired airspeed (m/s)

rosplane_msgs/srv/AddWaypoint.srv

@@ -1,7 +1,8 @@
 # Service to dynamically add new waypoint
 
 # @warning w and Va_d always have to be valid; the chi_d is optional.
-float32[3] w		# Waypoint in local NED (m)
+float32[3] w		# Waypoint in local NED (m) or LLA
+bool lla            # Set this flag true if the waypoint is given in LLA and not NED
 float32 chi_d		# Desired course at this waypoint (rad)
 bool use_chi		# Desired course valid (dubin or fillet paths)
 float32 va_d		# Desired airspeed (m/s)