18 #ifndef LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_
19 #define LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_
23 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
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>
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>
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>
49 #include <rclcpp/rclcpp.hpp>
52 #include <tf2_ros/transform_listener.h>
53 #include <tf2_ros/buffer.h>
58 #include <unordered_map>
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;
88 using VSC = autoware_auto_vehicle_msgs::msg::VehicleStateCommand;
89 using VSD = lgsvl_msgs::msg::VehicleStateData;
92 std::remove_const<decltype(autoware_auto_vehicle_msgs::msg::GearReport::DRIVE_1)>::type;
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,
120 bool update(std::chrono::nanoseconds timeout)
override;
123 bool send_state_command(
124 const autoware_auto_vehicle_msgs::msg::VehicleStateCommand & msg)
override;
129 bool send_control_command(
const autoware_auto_control_msgs::msg::AckermannControlCommand & msg)
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)
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;
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;
159 void on_gear_report(
const autoware_auto_vehicle_msgs::msg::GearReport & msg);
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{};
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{};
178 std::shared_ptr<tf2_ros::Buffer> m_tf_buffer;
179 std::shared_ptr<tf2_ros::TransformListener> m_tf_listener;
181 bool m_nav_base_tf_set{
false};
184 bool m_odom_set{
false};
185 geometry_msgs::msg::Vector3 m_odom_zero{};
187 lgsvl_msgs::msg::VehicleStateData m_lgsvl_state{};
189 rclcpp::Logger m_logger;
194 #endif // LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_