Add domain_bridge and waypoint_interface (#117)

* 1. add domain_bridge and a template yaml file
2. add a waypoint interface package for local and global planner interfacing

* Incremented airstack-dev image version to 0.5.4

docker/Dockerfile.airstack-dev

@@ -29,6 +29,7 @@ RUN apt update -y && apt install -y \
     ros-humble-image-view \
     ros-humble-topic-tools \
     ros-humble-grid-map \
+    ros-humble-domain-bridge \
     libcgal-dev 
 
 RUN /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh

docker/docker-compose.yaml

@@ -65,19 +65,19 @@ services:
 
   # ==============================================================================
   robot:
-    image: &airstack-dev-image airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.3
+    image: &airstack-dev-image airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.4
     build:
       context: ../
       dockerfile: docker/Dockerfile.airstack-dev
       tags:
         - *airstack-dev-image
     entrypoint: ""
     # we use tmux send-keys so that the session stays alive
-    command: >
-      bash -c "ssh service restart;
-      tmux new -d -s robot_bringup
-      && tmux send-keys -t robot_bringup 'ros2 launch robot_bringup robot.launch.xml' ENTER
-      && sleep infinity"
+    # command: >
+    #   bash -c "ssh service restart;
+    #   tmux new -d -s robot_bringup
+    #   && tmux send-keys -t robot_bringup 'ros2 launch robot_bringup robot.launch.xml' ENTER
+    #   && sleep infinity"
     # Interactive shell
     stdin_open: true # docker run -i
     tty: true # docker run -t

ros_ws/src/robot/autonomy/4_global/waypoint_interface/CMakeLists.txt

@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 3.8)
+project(waypoint_interface)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+add_executable(waypoint_interface_node src/waypoint_interface_node.cpp)
+
+# target_link_libraries(waypoint_interface_node rclcpp nav_msgs geometry_msgs)
+
+ament_target_dependencies(waypoint_interface_node
+  rclcpp
+  std_msgs
+  geometry_msgs
+  nav_msgs
+)
+
+install(TARGETS
+  waypoint_interface_node
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

ros_ws/src/robot/autonomy/4_global/waypoint_interface/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>waypoint_interface</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">airstation-04</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ros_ws/src/robot/autonomy/4_global/waypoint_interface/src/waypoint_interface_node.cpp

@@ -0,0 +1,119 @@
+#include <cmath>
+#include <optional>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int32.hpp"
+
+using std::placeholders::_1;
+
+class WaypointInterfaceNode : public rclcpp::Node {
+   public:
+    WaypointInterfaceNode() : Node("waypoint_interface") {
+        this->declare_parameter<double>("lookahead_time", 5.0);
+        lookahead_time_ = this->get_parameter("lookahead_time").as_double();
+
+        subscription_ref_ = this->create_subscription<nav_msgs::msg::Path>(
+            "global_plan_reference", 10,
+            std::bind(&WaypointInterfaceNode::global_plan_reference_callback, this, _1));
+        subscription_eta_ = this->create_subscription<nav_msgs::msg::Path>(
+            "global_plan_eta", 10,
+            std::bind(&WaypointInterfaceNode::global_plan_eta_callback, this, _1));
+
+        // Publisher for the current waypoint index
+        waypoint_index_publisher_ =
+            this->create_publisher<std_msgs::msg::Int32>("current_waypoint_index", 10);
+
+        timer_ = this->create_wall_timer(std::chrono::seconds(1),
+                                         std::bind(&WaypointInterfaceNode::update_position, this));
+    }
+
+   private:
+    void global_plan_reference_callback(const nav_msgs::msg::Path::SharedPtr msg) {
+        global_plan_reference_ = msg->poses;
+        RCLCPP_INFO(this->get_logger(), "Received global plan reference with sparse waypoints.");
+    }
+
+    void global_plan_eta_callback(const nav_msgs::msg::Path::SharedPtr msg) {
+        global_plan_eta_ = msg->poses;
+        RCLCPP_INFO(this->get_logger(), "Received global plan eta with dense waypoints.");
+    }
+
+    void update_position() {
+        this->get_parameter("lookahead_time", lookahead_time_);
+        RCLCPP_INFO(this->get_logger(), "Checking waypoints within lookahead time = %f",
+                    lookahead_time_);
+        track_waypoints(lookahead_time_);
+    }
+
+    void track_waypoints(double lookahead_time) {
+        if (global_plan_reference_.empty() || global_plan_eta_.empty()) {
+            RCLCPP_WARN(this->get_logger(), "Global plans not yet received.");
+            return;
+        }
+
+        rclcpp::Time current_time = this->get_clock()->now();
+        rclcpp::Time target_time = current_time + rclcpp::Duration::from_seconds(lookahead_time);
+
+        int last_close_waypoint_idx = -1;
+
+        for (const auto &pose_eta : global_plan_eta_) {
+            if (rclcpp::Time(pose_eta.header.stamp) > target_time) {
+                break;
+            }
+
+            for (size_t i = 0; i < global_plan_reference_.size(); ++i) {
+                if (is_close(pose_eta.pose.position, global_plan_reference_[i].pose.position)) {
+                    last_close_waypoint_idx = i;
+                }
+            }
+        }
+
+        std_msgs::msg::Int32 waypoint_index_msg;
+
+        if (last_close_waypoint_idx == -1 ||
+            last_close_waypoint_idx >= static_cast<int>(global_plan_reference_.size()) - 1) {
+            RCLCPP_INFO(this->get_logger(),
+                        "Robot is not within close range of any reference waypoints.");
+            waypoint_index_msg.data = -1;  // No waypoint in range
+        } else {
+            RCLCPP_INFO(this->get_logger(), "Robot is traveling between waypoints %d and %d.",
+                        last_close_waypoint_idx, last_close_waypoint_idx + 1);
+            waypoint_index_msg.data = last_close_waypoint_idx;
+        }
+
+        // Publish the current waypoint index
+        waypoint_index_publisher_->publish(waypoint_index_msg);
+    }
+
+    bool is_close(const geometry_msgs::msg::Point &p1, const geometry_msgs::msg::Point &p2) const {
+        double dist = distance(p1, p2);
+        return dist < close_range_threshold_;
+    }
+
+    double distance(const geometry_msgs::msg::Point &p1,
+                    const geometry_msgs::msg::Point &p2) const {
+        return std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2) +
+                         std::pow(p1.z - p2.z, 2));
+    }
+
+    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr subscription_ref_;
+    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr subscription_eta_;
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr waypoint_index_publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    std::vector<geometry_msgs::msg::PoseStamped> global_plan_reference_;
+    std::vector<geometry_msgs::msg::PoseStamped> global_plan_eta_;
+
+    double lookahead_time_;
+    const double close_range_threshold_ = 0.5;  // Distance threshold for "close range" in meters
+};
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<WaypointInterfaceNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

ros_ws/src/robot/robot_bringup/params/domain_bridge.yaml

@@ -0,0 +1,49 @@
+name: my_domain_bridge
+from_domain: 1
+to_domain: 2
+topics:
+  # Bridge "/foo/chatter" topic from doman ID 2 to domain ID 3
+  # Automatically detect QoS settings and default to 'keep_last' history with depth 10
+  robot_1/interface/mavros/state:
+    type: mavros_msgs/msg/State
+    from_domain: 1
+    to_domain: 2
+
+  robot_2/interface/mavros/state:
+    type: mavros_msgs/msg/State
+    from_domain: 2
+    to_domain: 1
+
+  # Bridge "/clock" topic from doman ID 2 to domain ID 3,
+  # Override durability to be 'volatile' and override depth to be 1
+  clock:
+    type: rosgraph_msgs/msg/Clock
+    qos:
+      durability: volatile
+      depth: 1
+
+  # Bridge "/clock" topic from doman ID 2 to domain ID 6
+  # Automatically detect QoS settings and override history to 'keep_all'
+  # clock:
+  #   type: rosgraph_msgs/msg/Clock
+  #   to_domain: 6
+  #   qos:
+  #     history: keep_all
+
+  # Bridge "/chitter" topic from domain ID 2 to domain ID 3 with the name "/chatter"
+  # chitter:
+  #   type: example_interfaces/msg/String
+  #   remap: chatter
+
+# services:
+  # Bridge "add_two_ints" service from domain ID 4 to domain ID 6
+  # add_two_ints:
+  #   type: example_interfaces/srv/AddTwoInts
+  #   from_domain: 4
+  #   to_domain: 6
+
+# actions:
+  # Bridge "fibonacci" action from domain ID 1 to domain ID 3
+  # fibonacci:
+  #   type: example_interfaces/action/Fibonacci
+  #   from_domain: 1