inspection_ros.hpp

Go to the documentation of this file.

#pragma once
 
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>
 
#include <chrono>
#include <cmath>
#include <functional>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <string>
 
#include "common_lib/competition_logic/mission_logic.hpp"
#include "custom_interfaces/msg/control_command.hpp"
#include "custom_interfaces/msg/operational_status.hpp"
#include "custom_interfaces/msg/wheel_rpm.hpp"
#include "functions/inspection_functions.hpp"
 
class InspectionMission : public rclcpp::Node {
private:
  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr _finish_client_;
  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr _emergency_client_;
  rclcpp::Publisher<custom_interfaces::msg::ControlCommand>::SharedPtr _control_command_publisher_;
 
  rclcpp::Subscription<custom_interfaces::msg::OperationalStatus>::SharedPtr
      _mission_signal_subscription_;
  rclcpp::Subscription<custom_interfaces::msg::WheelRPM>::SharedPtr _motor_rpm_subscription_;
  rclcpp::TimerBase::SharedPtr _timer_;
 
  // Not used now
  using WSSPolicy =
      message_filters::sync_policies::ApproximateTime<custom_interfaces::msg::WheelRPM,
                                                      custom_interfaces::msg::WheelRPM>;
 
  std::shared_ptr<message_filters::Synchronizer<WSSPolicy>> _sync_;
 
  rclcpp::TimerBase::SharedPtr
      _mission_end_timer_;  
  bool _mission_end_timer_started_ = false;
  bool _car_stopped_ = false;
  rclcpp::TimerBase::SharedPtr _main_callback_timer_;  
  rclcpp::Clock _clock_;
  rclcpp::Time _initial_time_;  
  InspectionFunctions _inspection_object_ = InspectionFunctions();
 
  bool _go_ = false;                                 
  common_lib::competition_logic::Mission _mission_;  
 
  double _motor_rpm_ = 0.0;  
 
  void end_of_mission();
 
  void handle_end_of_mission_response(
      rclcpp::Client<std_srvs::srv::Trigger>::SharedFuture future) const;
 
public:
  void mission_decider(custom_interfaces::msg::OperationalStatus::SharedPtr mission_signal);
 
  void inspection_script();
 
  void update_rpms_callback(const custom_interfaces::msg::WheelRPM& motor_rpm);
 
  void publish_controls(double throttle, double steering) const;
 
  InspectionMission();
};