Autoware.Auto
lgsvl_interface.hpp
Go to the documentation of this file.
1 // Copyright 2020-2021 the Autoware Foundation
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 //
15 // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
18 #ifndef LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_
19 #define LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_
20 
22 
23 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
24 
25 #include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
26 #include <autoware_auto_vehicle_msgs/msg/hand_brake_command.hpp>
27 #include <autoware_auto_vehicle_msgs/msg/hand_brake_report.hpp>
28 #include <autoware_auto_vehicle_msgs/msg/headlights_command.hpp>
29 #include <autoware_auto_vehicle_msgs/msg/horn_command.hpp>
30 #include <autoware_auto_vehicle_msgs/msg/wipers_command.hpp>
31 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
32 #include <autoware_auto_vehicle_msgs/msg/raw_control_command.hpp>
33 #include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
34 #include <autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp>
35 #include <autoware_auto_vehicle_msgs/msg/vehicle_state_report.hpp>
36 #include <autoware_auto_vehicle_msgs/srv/autonomy_mode_change.hpp>
37 
38 #include <lgsvl_msgs/msg/vehicle_odometry.hpp>
39 #include <lgsvl_msgs/msg/can_bus_data.hpp>
40 #include <lgsvl_msgs/msg/vehicle_control_data.hpp>
41 #include <lgsvl_msgs/msg/vehicle_state_data.hpp>
42 
43 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
44 #include <nav_msgs/msg/odometry.hpp>
45 #include <tf2_msgs/msg/tf_message.hpp>
46 
48 #include <rclcpp/rclcpp.hpp>
50 
51 #include <tf2_ros/transform_listener.h>
52 #include <tf2_ros/buffer.h>
53 
54 #include <chrono>
55 #include <memory>
56 #include <string>
57 #include <unordered_map>
58 #include <utility>
59 
60 namespace lgsvl_interface
61 {
62 
64 
65 // initialise default covariance for each measurement
66 // if simulator does not provide estimate of a state variable
67 // variance should be set high
68 constexpr static double COV_X_VAR = 0.1; // ros covariance array is float64 = double
69 constexpr static double COV_Y_VAR = 0.1;
70 constexpr static double COV_Z_VAR = 0.1;
71 constexpr static double COV_RX_VAR = 0.1;
72 constexpr static double COV_RY_VAR = 0.1;
73 constexpr static double COV_RZ_VAR = 0.1;
74 
75 // Covariance array index values
76 constexpr static int32_t COV_X = 0;
77 constexpr static int32_t COV_Y = 7;
78 constexpr static int32_t COV_Z = 14;
79 constexpr static int32_t COV_RX = 21;
80 constexpr static int32_t COV_RY = 28;
81 constexpr static int32_t COV_RZ = 35;
82 
83 constexpr bool PUBLISH = true;
84 constexpr bool NO_PUBLISH = false;
85 
86 // in lgsvl 0 is drive and 1 is reverse https://github.com/lgsvl/simulator/blob/cb937deb8e633573f6c0cc76c9f451398b8b9eff/Assets/Scripts/Sensors/VehicleStateSensor.cs#L70
87 using VSC = autoware_auto_vehicle_msgs::msg::VehicleStateCommand;
88 using VSD = lgsvl_msgs::msg::VehicleStateData;
89 using WIPER_TYPE = decltype(VSC::wiper);
90 using GEAR_TYPE =
91  std::remove_const<decltype(autoware_auto_vehicle_msgs::msg::GearReport::DRIVE_1)>::type;
92 using MODE_TYPE = decltype(VSC::mode);
93 
99 {
100 public:
102  rclcpp::Node & node,
103  const std::string & sim_cmd_topic,
104  const std::string & sim_state_cmd_topic,
105  const std::string & sim_state_report_topic,
106  const std::string & sim_nav_odom_topic,
107  const std::string & sim_veh_odom_topic,
108  const std::string & kinematic_state_topic,
109  const std::string & sim_odom_child_frame,
110  Table1D && throttle_table,
111  Table1D && brake_table,
112  Table1D && steer_table,
113  bool publish_tf = NO_PUBLISH,
114  bool publish_pose = PUBLISH);
115 
116  ~LgsvlInterface() noexcept override = default;
119  bool update(std::chrono::nanoseconds timeout) override;
122  bool send_state_command(
123  const autoware_auto_vehicle_msgs::msg::VehicleStateCommand & msg) override;
125  bool send_control_command(const autoware_auto_vehicle_msgs::msg::VehicleControlCommand & msg)
126  override;
128  bool send_control_command(const autoware_auto_control_msgs::msg::AckermannControlCommand & msg)
129  override;
132  bool send_control_command(
133  const autoware_auto_vehicle_msgs::msg::RawControlCommand & msg) override;
135  bool handle_mode_change_request(
136  autoware_auto_vehicle_msgs::srv::AutonomyModeChange_Request::SharedPtr request) override;
138  void send_headlights_command(const autoware_auto_vehicle_msgs::msg::HeadlightsCommand & msg)
139  override;
141  void send_horn_command(const autoware_auto_vehicle_msgs::msg::HornCommand & msg) override;
143  void send_wipers_command(const autoware_auto_vehicle_msgs::msg::WipersCommand & msg) override;
144 
145 private:
146  // Mappings from Autoware to LGSVL values
147  static const std::unordered_map<WIPER_TYPE, WIPER_TYPE> autoware_to_lgsvl_wiper;
148  static const std::unordered_map<GEAR_TYPE, GEAR_TYPE> autoware_to_lgsvl_gear;
149  static const std::unordered_map<GEAR_TYPE, GEAR_TYPE> vsc_to_lgsvl_gear;
150  static const std::unordered_map<MODE_TYPE, MODE_TYPE> autoware_to_lgsvl_mode;
151 
152  // Convert odometry into vehicle kinematic state and pose
153  void on_odometry(const nav_msgs::msg::Odometry & msg);
154 
155  // store gear_report with gear value correction
156  void on_gear_report(const autoware_auto_vehicle_msgs::msg::GearReport & msg);
157 
158  rclcpp::Publisher<lgsvl_msgs::msg::VehicleControlData>::SharedPtr m_cmd_pub{};
159  rclcpp::Publisher<lgsvl_msgs::msg::VehicleStateData>::SharedPtr m_state_pub{};
160  rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::VehicleKinematicState>::SharedPtr
161  m_kinematic_state_pub{};
162  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr m_tf_pub{};
163  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr m_pose_pub{};
164 
165  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr m_nav_odom_sub{};
166  rclcpp::Subscription<lgsvl_msgs::msg::CanBusData>::SharedPtr m_state_sub{};
167  rclcpp::Subscription<lgsvl_msgs::msg::VehicleOdometry>::SharedPtr m_veh_odom_sub{};
168  rclcpp::TimerBase::SharedPtr m_nav_base_tf_timer{};
169 
170  Table1D m_throttle_table;
171  Table1D m_brake_table;
172  Table1D m_steer_table;
173 
174  // transforms
175  std::shared_ptr<tf2_ros::Buffer> m_tf_buffer;
176  std::shared_ptr<tf2_ros::TransformListener> m_tf_listener;
177 
178  bool m_nav_base_tf_set{false};
179  autoware_auto_vehicle_msgs::msg::VehicleKinematicState m_nav_base_in_child_frame{};
180 
181  bool m_odom_set{false}; // TODO(c.ho) this should be optional<Vector3>
182  geometry_msgs::msg::Vector3 m_odom_zero{};
183 
184  lgsvl_msgs::msg::VehicleStateData m_lgsvl_state{};
185 
186  rclcpp::Logger m_logger;
187 }; // class LgsvlInterface
188 
189 } // namespace lgsvl_interface
190 
191 #endif // LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_
lgsvl_interface::COV_RY
constexpr static int32_t COV_RY
Definition: lgsvl_interface.hpp:80
lgsvl_interface::COV_RZ_VAR
constexpr static double COV_RZ_VAR
Definition: lgsvl_interface.hpp:73
lgsvl_interface::WIPER_TYPE
decltype(VSC::wiper) WIPER_TYPE
Definition: lgsvl_interface.hpp:89
autoware::motion::control::pure_pursuit::VehicleControlCommand
autoware_auto_vehicle_msgs::msg::VehicleControlCommand VehicleControlCommand
Definition: pure_pursuit.hpp:41
lgsvl_interface
Definition: lgsvl_interface.hpp:60
lgsvl_interface::COV_Z
constexpr static int32_t COV_Z
Definition: lgsvl_interface.hpp:78
lgsvl_interface::COV_Z_VAR
constexpr static double COV_Z_VAR
Definition: lgsvl_interface.hpp:70
lgsvl_interface::COV_X_VAR
constexpr static double COV_X_VAR
Definition: lgsvl_interface.hpp:68
lgsvl_interface::MODE_TYPE
decltype(VSC::mode) MODE_TYPE
Definition: lgsvl_interface.hpp:92
lgsvl_interface::PUBLISH
constexpr bool PUBLISH
Definition: lgsvl_interface.hpp:83
autoware::drivers::vehicle_interface::PlatformInterface
Definition: platform_interface.hpp:78
LGSVL_INTERFACE_PUBLIC
#define LGSVL_INTERFACE_PUBLIC
Definition: drivers/lgsvl_interface/include/lgsvl_interface/visibility_control.hpp:46
lgsvl_interface::COV_RY_VAR
constexpr static double COV_RY_VAR
Definition: lgsvl_interface.hpp:72
lgsvl_interface::VSC
autoware_auto_vehicle_msgs::msg::VehicleStateCommand VSC
Definition: lgsvl_interface.hpp:87
lgsvl_interface::LgsvlInterface
Definition: lgsvl_interface.hpp:97
lgsvl_interface::COV_Y_VAR
constexpr static double COV_Y_VAR
Definition: lgsvl_interface.hpp:69
lgsvl_interface::VSD
lgsvl_msgs::msg::VehicleStateData VSD
Definition: lgsvl_interface.hpp:88
lgsvl_interface::GEAR_TYPE
std::remove_const< decltype(autoware_auto_vehicle_msgs::msg::GearReport::DRIVE_1)>::type GEAR_TYPE
Definition: lgsvl_interface.hpp:91
lgsvl_interface::COV_RX_VAR
constexpr static double COV_RX_VAR
Definition: lgsvl_interface.hpp:71
platform_interface.hpp
Base class for vehicle "translator".
sys_info_node.node
node
Definition: sys_info_node.py:23
autoware::common::helper_functions::LookupTable1D< double >
lgsvl_interface::COV_RZ
constexpr static int32_t COV_RZ
Definition: lgsvl_interface.hpp:81
visibility_control.hpp
Visibility contorl.
lgsvl_interface::COV_RX
constexpr static int32_t COV_RX
Definition: lgsvl_interface.hpp:79
autoware::drivers::vehicle_interface::Odometry
autoware_auto_vehicle_msgs::msg::VehicleOdometry Odometry
Definition: safety_state_machine.hpp:50
lgsvl_interface::COV_X
constexpr static int32_t COV_X
Definition: lgsvl_interface.hpp:76
lgsvl_interface::COV_Y
constexpr static int32_t COV_Y
Definition: lgsvl_interface.hpp:77
lookup_table.hpp
This file contains a 1D linear lookup table implementation.
lgsvl_interface::NO_PUBLISH
constexpr bool NO_PUBLISH
Definition: lgsvl_interface.hpp:84
autoware::trajectory_spoofer::VehicleKinematicState
autoware_auto_vehicle_msgs::msg::VehicleKinematicState VehicleKinematicState
Definition: trajectory_spoofer.hpp:42