Branch data Line data Source code
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. 16 : : /// \file 17 : : /// \brief Implementation of interface for LGSVL simulator 18 : : #ifndef LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_ 19 : : #define LGSVL_INTERFACE__LGSVL_INTERFACE_HPP_ 20 : : 21 : : #include <lgsvl_interface/visibility_control.hpp> 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 : : 48 : : #include <geometry/lookup_table.hpp> 49 : : #include <rclcpp/rclcpp.hpp> 50 : : #include <vehicle_interface/platform_interface.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 : : 64 : : using Table1D = ::autoware::common::helper_functions::LookupTable1D<double>; 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 : : 95 : : /// Platform interface implementation for LGSVL. Bridges data to and from the simulator 96 : : /// where custom logic is required to get simulator data to adhere to ROS conventions. 97 : : /// For a full list of behaviors, see \ref lgsvl 98 : : class LGSVL_INTERFACE_PUBLIC LgsvlInterface 99 : : : public ::autoware::drivers::vehicle_interface::PlatformInterface 100 : : { 101 : : public: 102 : : LgsvlInterface( 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 [ + - ]: 15 : ~LgsvlInterface() noexcept override = default; 118 : : /// Receives data from ROS 2 subscriber, and updates output messages. 119 : : /// Not yet implemented 120 : : bool update(std::chrono::nanoseconds timeout) override; 121 : : /// Queues up data to be sent along with the next control command. 122 : : /// Only gear shifting between drive and reverse is supported at this time. 123 : : bool send_state_command( 124 : : const autoware_auto_vehicle_msgs::msg::VehicleStateCommand & msg) override; 125 : : /// Send control command data with whatever state data came along last 126 : : bool send_control_command(const autoware_auto_vehicle_msgs::msg::VehicleControlCommand & msg) 127 : : override; 128 : : /// Send control command data with whatever state data came along last 129 : : bool send_control_command(const autoware_auto_control_msgs::msg::AckermannControlCommand & msg) 130 : : override; 131 : : /// Send control data with whatever state data came along last; applies scaling here too. 132 : : /// If both brake and throttle is nonzero, decide based on config 133 : : bool send_control_command( 134 : : const autoware_auto_vehicle_msgs::msg::RawControlCommand & msg) override; 135 : : /// Respond to request for changing autonomy mode. For LGSVL, this means nothing. 136 : : bool handle_mode_change_request( 137 : : autoware_auto_vehicle_msgs::srv::AutonomyModeChange_Request::SharedPtr request) override; 138 : : /// Send gear command data. 139 : : void send_gear_command(const autoware_auto_vehicle_msgs::msg::GearCommand & msg) override; 140 : : /// Send headlights command data. 141 : : void send_headlights_command(const autoware_auto_vehicle_msgs::msg::HeadlightsCommand & msg) 142 : : override; 143 : : /// Send horn command data. 144 : : void send_horn_command(const autoware_auto_vehicle_msgs::msg::HornCommand & msg) override; 145 : : /// Send wipers command data. 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_