LCOV - code coverage report
Current view: top level - src/drivers/lgsvl_interface/include/lgsvl_interface - lgsvl_interface.hpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 1 1 100.0 %
Date: 2023-03-03 05:44:19 Functions: 2 2 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 1 2 50.0 %

           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_

Generated by: LCOV version 1.14