Xs1nus route follower

packages/route_follower/CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.8)
+project(route_follower)
+
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(truck_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(speed REQUIRED)
+find_package(collision REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+add_executable(${PROJECT_NAME}_node src/main.cpp src/route_follower_node.cpp)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}_node
+  rclcpp
+  truck_msgs
+  nav_msgs
+  geometry_msgs
+  speed
+  collision
+  tf2
+  tf2_ros
+  std_srvs
+  visualization_msgs
+)
+
+install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME})
+
+target_include_directories(
+  ${PROJECT_NAME}_node
+  PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+         "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
+         "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
+ament_package()

packages/route_follower/include/route_follower/route_follower_node.h

@@ -0,0 +1,90 @@
+#pragma once
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <std_srvs/srv/empty.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include "truck_msgs/msg/navigation_route.hpp"
+#include "motion/trajectory.h"
+#include "speed/greedy_planner.h"
+#include "collision/collision_checker.h"
+#include "geom/localization.h"
+#include "geom/transform.h"
+
+#include <rclcpp/time.hpp>
+#include <tf2_ros/qos.hpp>
+
+#include <chrono>
+#include <optional>
+#include <memory>
+
+namespace truck::route_follower {
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+class RouteFollowerNode : public rclcpp::Node {
+  public:
+    RouteFollowerNode();
+
+  private:
+    void onRoute(const truck_msgs::msg::NavigationRoute::SharedPtr msg);
+    void onOdometry(nav_msgs::msg::Odometry::SharedPtr msg);
+    void onGrid(nav_msgs::msg::OccupancyGrid::SharedPtr msg);
+    void onTf(tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static);
+
+    void onReset(
+        const std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr);
+
+    void publishGridCostMap();
+    void publishTrajectory();
+    void publishFullState();
+
+    std::optional<geom::Transform> getLatestTranform(
+        const std::string& source, const std::string& target);
+
+    struct Parameters {
+        std::chrono::duration<double> period = 0.1s;
+        double safety_margin = 0.3;
+    } params_;
+
+    speed::GreedyPlanner::Params speed_params_{};
+
+    struct Timers {
+        rclcpp::TimerBase::SharedPtr main = nullptr;
+    } timer_;
+
+    struct Services {
+        rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset = nullptr;
+    } service_;
+
+    struct Slots {
+        rclcpp::Subscription<truck_msgs::msg::NavigationRoute>::SharedPtr route = nullptr;
+        rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry = nullptr;
+        rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr grid = nullptr;
+        rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf = nullptr;
+        rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_static = nullptr;
+    } slot_;
+
+    struct Signals {
+        rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr distance_transform = nullptr;
+        rclcpp::Publisher<truck_msgs::msg::Trajectory>::SharedPtr trajectory = nullptr;
+    } signal_;
+
+    struct State {
+        nav_msgs::msg::Odometry::SharedPtr odometry = nullptr;
+        std::optional<geom::Localization> localization = std::nullopt;
+        nav_msgs::msg::OccupancyGrid::SharedPtr grid = nullptr;
+        std::shared_ptr<collision::Map> distance_transform = nullptr;
+        motion::Trajectory trajectory;
+        double scheduled_velocity = 0;
+    } state_;
+
+    std::unique_ptr<model::Model> model_ = nullptr;
+    std::unique_ptr<collision::StaticCollisionChecker> checker_ = nullptr;
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_ = nullptr;
+};
+
+}  // namespace truck::route_follower

packages/route_follower/launch/route_follower.yaml

@@ -0,0 +1,22 @@
+launch:
+- arg:
+    name: "model_config"
+    default: "$(find-pkg-share model)/config/model.yaml"
+- arg:
+    name: "simulation"
+    default: "false"
+- node:
+    pkg: "route_follower"
+    exec: "route_follower_node"
+    name: "route_follower_node"
+    output: "log"
+    param:
+    - { name: "use_sim_time", value: "$(var simulation)" }
+    - { name: "model_config", value: "$(var model_config)" }
+    - { name: "period", value: 0.1 }
+    - { name: "resolution", value: 0.02 }
+    - { name: "check_in_distance", value: 0.30 }
+    - { name: "safety_margin", value: 0.15 }
+    - { name: "distance_to_obstacle", value: 1.5 }
+    - { name: "acceleration/min", value: -0.5 }
+    - { name: "acceleration/max", value: 0.3 }

packages/route_follower/package.xml

@@ -0,0 +1,24 @@
+<?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>route_follower</name>
+  <version>0.0.0</version>
+  <description>route_follower</description>
+  <maintainer email="[email protected]">xs1nus</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>truck_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>speed</depend>
+  <depend>collision</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

packages/route_follower/src/main.cpp

@@ -0,0 +1,9 @@
+#include <rclcpp/rclcpp.hpp>
+#include "route_follower/route_follower_node.h"
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<truck::route_follower::RouteFollowerNode>());
+    rclcpp::shutdown();
+    return 0;
+}