ros_node.hpp

Go to the documentation of this file.

#pragma once
 
#include <chrono>
#include <functional>
#include <memory>
#include <string>
 
#include "config/parameters.hpp"
#include "custom_interfaces/msg/operational_status.hpp"
#include "custom_interfaces/msg/path_point_array.hpp"
#include "custom_interfaces/msg/pose.hpp"
#include "custom_interfaces/msg/velocities.hpp"
#include "utils/utils.hpp"
#include "rclcpp/rclcpp.hpp"
#include "control_solver/map.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
 
 
class ControlNode : public rclcpp::Node {
protected:
  bool go_signal_{false};
  ControlParameters params_;
 
  void vehicle_pose_callback(const custom_interfaces::msg::Pose &msg);
 
  void path_callback(const custom_interfaces::msg::PathPointArray &msg);
 
  void vehicle_state_callback(const custom_interfaces::msg::Velocities &msg);
private:
  // Vector of execution times for different parts of the control loop 
  // Currently just the first element is used, which is the total execution time
  std::shared_ptr<std::vector<double>> _execution_times_;
 
  // Map of topic to publisher pointers
  std::map<std::string, std::shared_ptr<rclcpp::PublisherBase>> publisher_map_;
 
  // Control solver (lateral + longitudinal)
  std::shared_ptr<ControlSolver> control_solver_;
 
  // Publishers
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr execution_time_pub_;
 
  // Timers
  rclcpp::TimerBase::SharedPtr control_timer_;
 
  // Subscriptions
  rclcpp::Subscription<custom_interfaces::msg::PathPointArray>::SharedPtr path_point_array_sub_;
  rclcpp::Subscription<custom_interfaces::msg::Pose>::SharedPtr vehicle_pose_sub_;
  rclcpp::Subscription<custom_interfaces::msg::Velocities>::SharedPtr velocity_sub_;
 
  void control_timer_callback();
 
  virtual void publish_command(common_lib::structures::ControlCommand cmd) = 0;
 
public:
  explicit ControlNode(const ControlParameters &params);
};