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_command.hpp>
26 #include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
27 #include <autoware_auto_vehicle_msgs/msg/hand_brake_command.hpp>
28 #include <autoware_auto_vehicle_msgs/msg/hand_brake_report.hpp>
29 #include <autoware_auto_vehicle_msgs/msg/headlights_command.hpp>
30 #include <autoware_auto_vehicle_msgs/msg/horn_command.hpp>
31 #include <autoware_auto_vehicle_msgs/msg/wipers_command.hpp>
32 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
33 #include <autoware_auto_vehicle_msgs/msg/raw_control_command.hpp>
34 #include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
35 #include <autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp>
36 #include <autoware_auto_vehicle_msgs/msg/vehicle_state_report.hpp>
37 #include <autoware_auto_vehicle_msgs/srv/autonomy_mode_change.hpp>
38 
39 #include <lgsvl_msgs/msg/vehicle_odometry.hpp>
40 #include <lgsvl_msgs/msg/can_bus_data.hpp>
41 #include <lgsvl_msgs/msg/vehicle_control_data.hpp>
42 #include <lgsvl_msgs/msg/vehicle_state_data.hpp>
43 
44 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
45 #include <nav_msgs/msg/odometry.hpp>
46 #include <tf2_msgs/msg/tf_message.hpp>
47 
49 #include <rclcpp/rclcpp.hpp>
51 
52 #include <tf2_ros/transform_listener.h>
53 #include <tf2_ros/buffer.h>
54 
55 #include <chrono>
56 #include <memory>
57 #include <string>
58 #include <unordered_map>
59 #include <utility>
60 
61 namespace lgsvl_interface
62 {
63 
65 
66 // initialise default covariance for each measurement
67 // if simulator does not provide estimate of a state variable
68 // variance should be set high
69 constexpr static double COV_X_VAR = 0.1; // ros covariance array is float64 = double
70 constexpr static double COV_Y_VAR = 0.1;
71 constexpr static double COV_Z_VAR = 0.1;
72 constexpr static double COV_RX_VAR = 0.1;
73 constexpr static double COV_RY_VAR = 0.1;
74 constexpr static double COV_RZ_VAR = 0.1;
75 
76 // Covariance array index values
77 constexpr static int32_t COV_X = 0;
78 constexpr static int32_t COV_Y = 7;
79 constexpr static int32_t COV_Z = 14;
80 constexpr static int32_t COV_RX = 21;
81 constexpr static int32_t COV_RY = 28;
82 constexpr static int32_t COV_RZ = 35;
83 
84 constexpr bool PUBLISH = true;
85 constexpr bool NO_PUBLISH = false;
86 
87 // in lgsvl 0 is drive and 1 is reverse https://github.com/lgsvl/simulator/blob/cb937deb8e633573f6c0cc76c9f451398b8b9eff/Assets/Scripts/Sensors/VehicleStateSensor.cs#L70
88 using VSC = autoware_auto_vehicle_msgs::msg::VehicleStateCommand;
89 using VSD = lgsvl_msgs::msg::VehicleStateData;
90 using WIPER_TYPE = decltype(VSC::wiper);
91 using GEAR_TYPE =
92  std::remove_const<decltype(autoware_auto_vehicle_msgs::msg::GearReport::DRIVE_1)>::type;
93 using MODE_TYPE = decltype(VSC::mode);
94 
100 {
101 public:
103  rclcpp::Node & node,
104  const std::string & sim_cmd_topic,
105  const std::string & sim_state_cmd_topic,
106  const std::string & sim_state_report_topic,
107  const std::string & sim_nav_odom_topic,
108  const std::string & sim_veh_odom_topic,
109  const std::string & kinematic_state_topic,
110  const std::string & sim_odom_child_frame,
111  Table1D && throttle_table,
112  Table1D && brake_table,
113  Table1D && steer_table,
114  bool publish_tf = NO_PUBLISH,
115  bool publish_pose = PUBLISH);
116 
117  ~LgsvlInterface() noexcept override = default;
120  bool update(std::chrono::nanoseconds timeout) override;
123  bool send_state_command(
124  const autoware_auto_vehicle_msgs::msg::VehicleStateCommand & msg) override;
126  bool send_control_command(const autoware_auto_vehicle_msgs::msg::VehicleControlCommand & msg)
127  override;
129  bool send_control_command(const autoware_auto_control_msgs::msg::AckermannControlCommand & msg)
130  override;
133  bool send_control_command(
134  const autoware_auto_vehicle_msgs::msg::RawControlCommand & msg) override;
136  bool handle_mode_change_request(
137  autoware_auto_vehicle_msgs::srv::AutonomyModeChange_Request::SharedPtr request) override;
139  void send_gear_command(const autoware_auto_vehicle_msgs::msg::GearCommand & msg) override;
141  void send_headlights_command(const autoware_auto_vehicle_msgs::msg::HeadlightsCommand & msg)
142  override;
144  void send_horn_command(const autoware_auto_vehicle_msgs::msg::HornCommand & msg) override;
146  void send_wipers_command(const autoware_auto_vehicle_msgs::msg::WipersCommand & msg) override;
147 
148 private:
149  // Mappings from Autoware to LGSVL values
150  static const std::unordered_map<WIPER_TYPE, WIPER_TYPE> autoware_to_lgsvl_wiper;
151  static const std::unordered_map<GEAR_TYPE, GEAR_TYPE> autoware_to_lgsvl_gear;
152  static const std::unordered_map<GEAR_TYPE, GEAR_TYPE> vsc_to_lgsvl_gear;
153  static const std::unordered_map<MODE_TYPE, MODE_TYPE> autoware_to_lgsvl_mode;
154 
155  // Convert odometry into vehicle kinematic state and pose
156  void on_odometry(const nav_msgs::msg::Odometry & msg);
157 
158  // store gear_report with gear value correction
159  void on_gear_report(const autoware_auto_vehicle_msgs::msg::GearReport & msg);
160 
161  rclcpp::Publisher<lgsvl_msgs::msg::VehicleControlData>::SharedPtr m_cmd_pub{};
162  rclcpp::Publisher<lgsvl_msgs::msg::VehicleStateData>::SharedPtr m_state_pub{};
163  rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::VehicleKinematicState>::SharedPtr
164  m_kinematic_state_pub{};
165  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr m_tf_pub{};
166  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr m_pose_pub{};
167 
168  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr m_nav_odom_sub{};
169  rclcpp::Subscription<lgsvl_msgs::msg::CanBusData>::SharedPtr m_state_sub{};
170  rclcpp::Subscription<lgsvl_msgs::msg::VehicleOdometry>::SharedPtr m_veh_odom_sub{};
171  rclcpp::TimerBase::SharedPtr m_nav_base_tf_timer{};
172 
173  Table1D m_throttle_table;
174  Table1D m_brake_table;
175  Table1D m_steer_table;
176 
177  // transforms
178  std::shared_ptr<tf2_ros::Buffer> m_tf_buffer;
179  std::shared_ptr<tf2_ros::TransformListener> m_tf_listener;
180 
181  bool m_nav_base_tf_set{false};
182  autoware_auto_vehicle_msgs::msg::VehicleKinematicState m_nav_base_in_child_frame{};
183 
184  bool m_odom_set{false}; // TODO(c.ho) this should be optional<Vector3>
185  geometry_msgs::msg::Vector3 m_odom_zero{};
186 
187  lgsvl_msgs::msg::VehicleStateData m_lgsvl_state{};
188 
189  rclcpp::Logger m_logger;
190 }; // class LgsvlInterface
191 
192 } // namespace lgsvl_interface
193 
194 #endif // LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_
lgsvl_interface::COV_RY
constexpr static int32_t COV_RY
Definition: lgsvl_interface.hpp:81
lgsvl_interface::COV_RZ_VAR
constexpr static double COV_RZ_VAR
Definition: lgsvl_interface.hpp:74
lgsvl_interface::WIPER_TYPE
decltype(VSC::wiper) WIPER_TYPE
Definition: lgsvl_interface.hpp:90
autoware::motion::control::pure_pursuit::VehicleControlCommand
autoware_auto_vehicle_msgs::msg::VehicleControlCommand VehicleControlCommand
Definition: pure_pursuit.hpp:41
lgsvl_interface
Definition: lgsvl_interface.hpp:61
lgsvl_interface::COV_Z
constexpr static int32_t COV_Z
Definition: lgsvl_interface.hpp:79
lgsvl_interface::COV_Z_VAR
constexpr static double COV_Z_VAR
Definition: lgsvl_interface.hpp:71
lgsvl_interface::COV_X_VAR
constexpr static double COV_X_VAR
Definition: lgsvl_interface.hpp:69
lgsvl_interface::MODE_TYPE
decltype(VSC::mode) MODE_TYPE
Definition: lgsvl_interface.hpp:93
lgsvl_interface::PUBLISH
constexpr bool PUBLISH
Definition: lgsvl_interface.hpp:84
autoware::drivers::vehicle_interface::PlatformInterface
Definition: platform_interface.hpp:80
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:73
lgsvl_interface::VSC
autoware_auto_vehicle_msgs::msg::VehicleStateCommand VSC
Definition: lgsvl_interface.hpp:88
lgsvl_interface::LgsvlInterface
Definition: lgsvl_interface.hpp:98
lgsvl_interface::COV_Y_VAR
constexpr static double COV_Y_VAR
Definition: lgsvl_interface.hpp:70
lgsvl_interface::VSD
lgsvl_msgs::msg::VehicleStateData VSD
Definition: lgsvl_interface.hpp:89
lgsvl_interface::GEAR_TYPE
std::remove_const< decltype(autoware_auto_vehicle_msgs::msg::GearReport::DRIVE_1)>::type GEAR_TYPE
Definition: lgsvl_interface.hpp:92
lgsvl_interface::COV_RX_VAR
constexpr static double COV_RX_VAR
Definition: lgsvl_interface.hpp:72
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:82
visibility_control.hpp
Visibility contorl.
lgsvl_interface::COV_RX
constexpr static int32_t COV_RX
Definition: lgsvl_interface.hpp:80
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:77
lgsvl_interface::COV_Y
constexpr static int32_t COV_Y
Definition: lgsvl_interface.hpp:78
lookup_table.hpp
This file contains a 1D linear lookup table implementation.
lgsvl_interface::NO_PUBLISH
constexpr bool NO_PUBLISH
Definition: lgsvl_interface.hpp:85
autoware::trajectory_spoofer::VehicleKinematicState
autoware_auto_vehicle_msgs::msg::VehicleKinematicState VehicleKinematicState
Definition: trajectory_spoofer.hpp:42