lgsvl_interface.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 LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_
#define LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_

#include <lgsvl_interface/visibility_control.hpp>

#include <autoware_auto_msgs/msg/raw_control_command.hpp>
#include <autoware_auto_msgs/msg/vehicle_kinematic_state.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 <lgsvl_msgs/msg/vehicle_odometry.hpp>
#include <lgsvl_msgs/msg/can_bus_data.hpp>
#include <lgsvl_msgs/msg/vehicle_control_data.hpp>
#include <lgsvl_msgs/msg/vehicle_state_data.hpp>

#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2_msgs/msg/tf_message.hpp>

#include <geometry/lookup_table.hpp>
#include <rclcpp/rclcpp.hpp>
#include <vehicle_interface/platform_interface.hpp>

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>

#include <chrono>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>

namespace lgsvl_interface
{

using Table1D = ::autoware::common::helper_functions::LookupTable1D<double>;

// initialise default covariance for each measurement
// if simulator does not provide estimate of a state variable
// variance should be set high
constexpr static double COV_X_VAR = 0.1;  // ros covariance array is float64 = double
constexpr static double COV_Y_VAR = 0.1;
constexpr static double COV_Z_VAR = 0.1;
constexpr static double COV_RX_VAR = 1000.0;
constexpr static double COV_RY_VAR = 1000.0;
constexpr static double COV_RZ_VAR = 1000.0;

// Covariance array index values
constexpr static int32_t COV_X = 0;
constexpr static int32_t COV_Y = 7;
constexpr static int32_t COV_Z = 14;
constexpr static int32_t COV_RX = 21;
constexpr static int32_t COV_RY = 28;
constexpr static int32_t COV_RZ = 35;

constexpr bool PUBLISH = true;
constexpr bool NO_PUBLISH = false;

// in lgsvl 0 is drive and 1 is reverse https://github.com/lgsvl/simulator/blob/cb937deb8e633573f6c0cc76c9f451398b8b9eff/Assets/Scripts/Sensors/VehicleStateSensor.cs#L70
using VSC = autoware_auto_msgs::msg::VehicleStateCommand;
using VSD = lgsvl_msgs::msg::VehicleStateData;
using WIPER_TYPE = decltype(VSC::wiper);
using GEAR_TYPE = decltype(VSC::gear);
using MODE_TYPE = decltype(VSC::mode);

class LGSVL_INTERFACE_PUBLIC LgsvlInterface
  : public ::autoware::drivers::vehicle_interface::PlatformInterface
{
public:
  LgsvlInterface(
    rclcpp::Node & node,
    const std::string & sim_cmd_topic,
    const std::string & sim_state_cmd_topic,
    const std::string & sim_state_report_topic,
    const std::string & sim_nav_odom_topic,
    const std::string & sim_veh_odom_topic,
    const std::string & kinematic_state_topic,
    const std::string & sim_odom_child_frame,
    Table1D && throttle_table,
    Table1D && brake_table,
    Table1D && steer_table,
    bool publish_tf = NO_PUBLISH,
    bool publish_pose = PUBLISH);

  ~LgsvlInterface() noexcept override = default;
  bool update(std::chrono::nanoseconds timeout) override;
  bool send_state_command(const autoware_auto_msgs::msg::VehicleStateCommand & msg) override;
  bool send_control_command(const autoware_auto_msgs::msg::VehicleControlCommand & msg) override;
  bool send_control_command(const autoware_auto_msgs::msg::RawControlCommand & msg) override;
  bool handle_mode_change_request(
    autoware_auto_msgs::srv::AutonomyModeChange_Request::SharedPtr request) override;

private:
  // Mappings from Autoware to LGSVL values
  static const std::unordered_map<WIPER_TYPE, WIPER_TYPE> autoware_to_lgsvl_wiper;
  static const std::unordered_map<GEAR_TYPE, GEAR_TYPE> autoware_to_lgsvl_gear;
  static const std::unordered_map<MODE_TYPE, MODE_TYPE> autoware_to_lgsvl_mode;

  // Convert odometry into vehicle kinematic state and pose
  void on_odometry(const nav_msgs::msg::Odometry & msg);

  // store state_report with gear value correction
  void on_state_report(const autoware_auto_msgs::msg::VehicleStateReport & msg);

  rclcpp::Publisher<lgsvl_msgs::msg::VehicleControlData>::SharedPtr m_cmd_pub{};
  rclcpp::Publisher<lgsvl_msgs::msg::VehicleStateData>::SharedPtr m_state_pub{};
  rclcpp::Publisher<autoware_auto_msgs::msg::VehicleKinematicState>::SharedPtr
    m_kinematic_state_pub{};
  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr m_tf_pub{};
  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr m_pose_pub{};
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr m_nav_odom_sub{};
  rclcpp::Subscription<lgsvl_msgs::msg::CanBusData>::SharedPtr m_state_sub{};
  rclcpp::Subscription<lgsvl_msgs::msg::VehicleOdometry>::SharedPtr m_veh_odom_sub{};
  rclcpp::TimerBase::SharedPtr m_nav_base_tf_timer{};

  Table1D m_throttle_table;
  Table1D m_brake_table;
  Table1D m_steer_table;

  // transforms
  std::shared_ptr<tf2_ros::Buffer> m_tf_buffer;
  std::shared_ptr<tf2_ros::TransformListener> m_tf_listener;

  bool m_nav_base_tf_set{false};
  autoware_auto_msgs::msg::VehicleKinematicState m_nav_base_in_child_frame{};

  bool m_odom_set{false};  // TODO(c.ho) this should be optional<Vector3>
  geometry_msgs::msg::Vector3 m_odom_zero{};

  rclcpp::Logger m_logger;
};  // class LgsvlInterface

}  // namespace lgsvl_interface

#endif  // LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_