vehicle_interface_node.hpp

Go to the documentation of this file.

// Copyright 2020 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
#ifndef VEHICLE_INTERFACE__VEHICLE_INTERFACE_NODE_HPP_
#define VEHICLE_INTERFACE__VEHICLE_INTERFACE_NODE_HPP_

#include <vehicle_interface/platform_interface.hpp>
#include <vehicle_interface/safety_state_machine.hpp>
#include <vehicle_interface/visibility_control.hpp>

#include <mpark_variant_vendor/variant.hpp>
#include <rclcpp/rclcpp.hpp>
#include <reference_tracking_controller/reference_tracking_controller.hpp>
#include <signal_filters/signal_filter.hpp>

#include <autoware_auto_msgs/msg/high_level_control_command.hpp>
#include <autoware_auto_msgs/msg/raw_control_command.hpp>
#include <autoware_auto_msgs/msg/vehicle_control_command.hpp>
#include <autoware_auto_msgs/msg/vehicle_odometry.hpp>
#include <autoware_auto_msgs/msg/vehicle_state_command.hpp>
#include <autoware_auto_msgs/msg/vehicle_state_report.hpp>
#include <autoware_auto_msgs/srv/autonomy_mode_change.hpp>

#include <experimental/optional>
#include <chrono>
#include <exception>
#include <memory>
#include <string>

namespace autoware
{
namespace drivers
{
namespace vehicle_interface
{
using Real = decltype(BasicControlCommand::long_accel_mps2);

struct TopicNumMatches
{
  std::string topic;
};  // struct TopicNumMatches

struct FilterConfig
{
  std::string type;
  Real cutoff_frequency;
};  // struct FilterConfig

class VEHICLE_INTERFACE_PUBLIC VehicleInterfaceNode : public ::rclcpp::Node
{
public:
  VehicleInterfaceNode(
    const std::string & node_name,
    const rclcpp::NodeOptions & options);

protected:
  using ControllerBasePtr =
    std::unique_ptr<common::reference_tracking_controller::ReferenceTrackerBase<Real>>;
  using FilterBasePtr = std::unique_ptr<common::signal_filters::FilterBase<Real>>;
  using ModeChangeRequest = autoware_auto_msgs::srv::AutonomyModeChange_Request;
  using ModeChangeResponse = autoware_auto_msgs::srv::AutonomyModeChange_Response;

  struct VehicleFilter
  {
    FilterBasePtr longitudinal;
    FilterBasePtr curvature;
    FilterBasePtr front_steer;
    FilterBasePtr rear_steer;
  };
  void set_filter(VehicleFilter && filter) noexcept;
  void set_reference_tracker(ControllerBasePtr && controller) noexcept;
  void set_interface(std::unique_ptr<PlatformInterface> && interface) noexcept;
  rclcpp::Logger logger() const noexcept;
  const SafetyStateMachine get_state_machine() const noexcept;

  virtual void on_control_send_failure();
  virtual void on_state_send_failure();
  virtual void on_mode_change_failure();
  virtual void on_read_timeout();
  virtual void on_error(std::exception_ptr eptr);

private:
  // Helper function called in constructors
  VEHICLE_INTERFACE_LOCAL void init(
    const TopicNumMatches & control_command,
    const TopicNumMatches & state_command,
    const TopicNumMatches & odometry,
    const TopicNumMatches & state_report,
    const std::experimental::optional<StateMachineConfig> & state_machine_config,
    const FilterConfig & longitudinal_filter,
    const FilterConfig & curvature_filter,
    const FilterConfig & front_steer_filter,
    const FilterConfig & rear_steer_filter,
    const std::chrono::nanoseconds & cycle_time);

  // Run just before main loop, ensure that all invariants (possibly from child class) are enforced
  VEHICLE_INTERFACE_LOCAL void check_invariants();

  // Send state command
  VEHICLE_INTERFACE_LOCAL void send_state_command(const MaybeStateCommand & maybe_command);
  // Read data from vehicle platform for time budget, publish data
  VEHICLE_INTERFACE_LOCAL void read_and_publish();
  // Core loop for different input commands. Specialized differently for each topic type
  template<typename T>
  VEHICLE_INTERFACE_LOCAL void on_command_message(const T & msg);
  // Callback for requests to change autonomoy mode
  VEHICLE_INTERFACE_LOCAL void on_mode_change_request(
    ModeChangeRequest::SharedPtr request, ModeChangeResponse::SharedPtr response);
  VEHICLE_INTERFACE_LOCAL void state_machine_report();

  rclcpp::TimerBase::SharedPtr m_read_timer{nullptr};
  rclcpp::Publisher<autoware_auto_msgs::msg::VehicleOdometry>::SharedPtr m_odom_pub{nullptr};
  rclcpp::Publisher<autoware_auto_msgs::msg::VehicleStateReport>::SharedPtr m_state_pub{nullptr};
  rclcpp::Subscription<autoware_auto_msgs::msg::VehicleStateCommand>::SharedPtr m_state_sub{};
  rclcpp::Service<autoware_auto_msgs::srv::AutonomyModeChange>::SharedPtr m_mode_service{nullptr};

  using BasicSub = rclcpp::Subscription<BasicControlCommand>::SharedPtr;
  using RawSub = rclcpp::Subscription<autoware_auto_msgs::msg::RawControlCommand>::SharedPtr;
  using HighLevelSub =
    rclcpp::Subscription<autoware_auto_msgs::msg::HighLevelControlCommand>::SharedPtr;

  mpark::variant<RawSub, BasicSub, HighLevelSub> m_command_sub{};

  std::unique_ptr<PlatformInterface> m_interface{nullptr};
  VehicleFilter m_filter{nullptr, nullptr, nullptr, nullptr};
  ControllerBasePtr m_controller{nullptr};
  std::unique_ptr<SafetyStateMachine> m_state_machine{nullptr};
  std::chrono::system_clock::time_point m_last_command_stamp{};
  std::chrono::nanoseconds m_cycle_time{};
  MaybeStateCommand m_last_state_command{};
};  // class VehicleInterfaceNode

}  // namespace vehicle_interface
}  // namespace drivers
}  // namespace autoware

#endif  // VEHICLE_INTERFACE__VEHICLE_INTERFACE_NODE_HPP_