|
Formula Student Autonomous Systems
The code for the main driverless system
|
Adapter for interfacing with the real vehicle hardware. More...
#include <vehicle.hpp>


Public Member Functions | |
| VehicleAdapter (const ControlParameters ¶ms) | |
| void | publish_command (common_lib::structures::ControlCommand cmd) override |
| Adapters override this function to publish control commands in their environment. | |
| void | go_signal_callback (const custom_interfaces::msg::OperationalStatus msg) |
| VehicleAdapter (std::shared_ptr< SENode > se_node) | |
| void | finish () final |
| Function that sends the finish signal to the respective node. | |
| VehicleAdapter (const PlanningParameters ¶ms) | |
| void | mission_state_callback (const custom_interfaces::msg::OperationalStatus &msg) |
| void | finish () override |
| Function that sends the finish signal to the respective node. | |
| VehicleAdapter (const SLAMParameters ¶ms) | |
| Constructor of the vehicle adapter node. | |
| void | _lidar_odometry_subscription_callback (const nav_msgs::msg::Odometry &msg) |
| LiDAR odometry subscription callback. | |
| void | finish () override |
| Sends the finished signal to the vehicle control. | |
| VehicleAdapter (const VEParameters ¶meters) | |
| void | imu_callback (const geometry_msgs::msg::Vector3Stamped::SharedPtr &free_acceleration_msg, const geometry_msgs::msg::Vector3Stamped::SharedPtr &angular_velocity_msg) |
| IMU subscription callback, which receives both free acceleration and angular velocity through a synchronizer message filter. | |
| void | wss_callback (const custom_interfaces::msg::WheelRPM &rl_wheel_rpm_msg, const custom_interfaces::msg::WheelRPM &rr_wheel_rpm_msg) |
| Wheel speed subscription callback, which receives both wheel speeds through a synchronizer message filter. | |
| void | steering_angle_callback (const custom_interfaces::msg::SteeringAngle msg) |
| Callback for the subscription of the steering angle sensor. | |
| void | resolver_callback (custom_interfaces::msg::WheelRPM msg) |
| Callback for the subscription of the resolver which measures the motor RPM. | |
Public Member Functions inherited from ControlNode | |
| ControlNode (const ControlParameters ¶ms) | |
Public Member Functions inherited from Adapter | |
| Adapter (std::shared_ptr< SENode > se_node) | |
| void | imu_subscription_callback (const sensor_msgs::msg::Imu &msg) |
| Function that parses the message sent from the ros IMU topic and calls the respective node's function to update the motion. | |
| virtual | ~Adapter ()=default |
Public Member Functions inherited from Planning | |
| Planning (const PlanningParameters ¶ms) | |
| Constructs a Planning node with the specified configuration parameters. | |
| void | set_mission (Mission mission) |
| Sets the mission type for planning execution. | |
Public Member Functions inherited from SLAMNode | |
| SLAMNode (const SLAMParameters ¶ms) | |
| Constructor that uses the parameters structure. | |
| void | init () |
| Initialize functions @description This method is used to initialize things that require the constructed node. | |
Public Member Functions inherited from VENode | |
| VENode (const VEParameters ¶meters) | |
Private Types | |
| using | ImuPolicy = message_filters::sync_policies::ApproximateTime< custom_interfaces::msg::ImuData, custom_interfaces::msg::ImuData > |
| Policy for synchronizing IMU data. | |
| using | XsensImuPolicy = message_filters::sync_policies::ApproximateTime< geometry_msgs::msg::Vector3Stamped, geometry_msgs::msg::Vector3Stamped > |
| Policy for synchronizing Xsens IMU data. | |
| using | WheelSteerPolicy = message_filters::sync_policies::ApproximateTime< custom_interfaces::msg::WheelRPM, custom_interfaces::msg::WheelRPM, custom_interfaces::msg::SteeringAngle > |
| Policy for synchronizing wheel speeds and steering angle. | |
| using | XsensImuPolicy = message_filters::sync_policies::ApproximateTime< geometry_msgs::msg::Vector3Stamped, geometry_msgs::msg::Vector3Stamped > |
| Policy for synchronizing Xsens IMU data. | |
| using | WheelSSPolicy = message_filters::sync_policies::ApproximateTime< custom_interfaces::msg::WheelRPM, custom_interfaces::msg::WheelRPM > |
| Policy for synchronizing wheel speeds. | |
Private Member Functions | |
| void | wheel_speeds_subscription_callback (const custom_interfaces::msg::WheelRPM &rl_wheel_rpm_msg, const custom_interfaces::msg::WheelRPM &rr_wheel_rpm_msg, const custom_interfaces::msg::SteeringAngle &steering_angle_msg) |
| Wheel speed subscription callback, which receives both wheel speeds and steering angle through a synchronizer message filter. | |
| void | imu_subscription_callback (const custom_interfaces::msg::ImuData &roll_accx_data, const custom_interfaces::msg::ImuData &yaw_accy_data) |
| IMU subscriptio callback, which receives both roll and yaw acceleration data separately and synchronizes them. | |
| void | xsens_imu_subscription_callback (const geometry_msgs::msg::Vector3Stamped::SharedPtr &free_acceleration_msg, const geometry_msgs::msg::Vector3Stamped::SharedPtr &angular_velocity_msg) |
| Xsens IMU subscription callback, which receives both free acceleration and angular velocity data separately and synchronizes them to then call State Estimation node's IMU callback. | |
Private Attributes | |
| rclcpp::Subscription< custom_interfaces::msg::OperationalStatus >::SharedPtr | go_sub_ |
| Subscription for the go signal from the operational status, which activates the controller when true. | |
| rclcpp::Publisher< custom_interfaces::msg::ControlCommand >::SharedPtr | control_pub_ |
| message_filters::Subscriber< custom_interfaces::msg::WheelRPM > | _rl_wheel_rpm_subscription_ |
| Subscriber for rl wheel rpm. | |
| message_filters::Subscriber< custom_interfaces::msg::WheelRPM > | _rr_wheel_rpm_subscription_ |
| Subscriber for rr wheel rpm. | |
| message_filters::Subscriber< custom_interfaces::msg::SteeringAngle > | _steering_angle_subscription_ |
| Subscriber for steering angle. | |
| rclcpp::Subscription< custom_interfaces::msg::OperationalStatus >::SharedPtr | _operational_status_subscription_ |
| Subscriber for operational status. | |
| message_filters::Subscriber< geometry_msgs::msg::Vector3Stamped > | _free_acceleration_subscription_ |
| message_filters::Subscriber< geometry_msgs::msg::Vector3Stamped > | _angular_velocity_subscription_ |
| message_filters::Subscriber< custom_interfaces::msg::ImuData > | _roll_accx_imu_subscription_ |
| message_filters::Subscriber< custom_interfaces::msg::ImuData > | _yaw_accy_imu_subscription_ |
| std::shared_ptr< message_filters::Synchronizer< WheelSteerPolicy > > | _sync_ |
| Synchronizer for wheel speeds and steering angle. | |
| std::shared_ptr< message_filters::Synchronizer< ImuPolicy > > | _imu_sync_ |
| std::shared_ptr< message_filters::Synchronizer< XsensImuPolicy > > | _xsens_imu_sync_ |
| rclcpp::Client< std_srvs::srv::Trigger >::SharedPtr | _finished_client_ |
| Client for finished signal. | |
| rclcpp::Subscription< custom_interfaces::msg::OperationalStatus >::SharedPtr | mission_subscription_ |
| rclcpp::Subscription< nav_msgs::msg::Odometry >::SharedPtr | _lidar_odometry_subscription_ |
| std::shared_ptr< tf2_ros::StaticTransformBroadcaster > | _tf_static_broadcaster_ |
| Static transform broadcaster. | |
| message_filters::Subscriber< custom_interfaces::msg::WheelRPM > | _fl_wheel_rpm_subscription_ |
| Subscriber for fl wheel rpm. | |
| message_filters::Subscriber< custom_interfaces::msg::WheelRPM > | _fr_wheel_rpm_subscription_ |
| Subscriber for fr wheel rpm. | |
| std::shared_ptr< message_filters::Synchronizer< WheelSSPolicy > > | _wss_sync_ |
| Synchronizer for wheel speeds. | |
| rclcpp::Subscription< custom_interfaces::msg::SteeringAngle >::SharedPtr | _steering_angle_sub_ |
| rclcpp::Subscription< custom_interfaces::msg::WheelRPM >::SharedPtr | _resolver_sub_ |
| double | last_decent_fl_wss_reading = 0 |
| double | average_imu_bias = 0.0 |
| int | number_of_imu_readings = 0 |
Additional Inherited Members | |
Static Public Member Functions inherited from Planning | |
| static PlanningParameters | load_config (std::string &adapter) |
| Loads planning configuration from YAML files. | |
Public Attributes inherited from Adapter | |
| std::shared_ptr< SENode > | node_ |
| rclcpp::Subscription< sensor_msgs::msg::Imu >::SharedPtr | _imu_subscription_ |
Protected Member Functions inherited from ControlNode | |
| void | vehicle_pose_callback (const custom_interfaces::msg::Pose &msg) |
| Called when a new vehicle pose is received. | |
| void | path_callback (const custom_interfaces::msg::PathPointArray &msg) |
| Called when a new path is received. | |
| void | vehicle_state_callback (const custom_interfaces::msg::Velocities &msg) |
| Called when a new velocity is received. | |
Protected Member Functions inherited from SLAMNode | |
| void | _perception_subscription_callback (const custom_interfaces::msg::PerceptionOutput &msg) |
| Callback that updates everytime information is received from the perception module. | |
| void | _velocities_subscription_callback (const custom_interfaces::msg::Velocities &msg) |
| Callback that updates everytime information is received from velocity estimation node. | |
| void | _imu_subscription_callback (const sensor_msgs::msg::Imu &msg) |
| Callback that updates everytime information is received from the IMU. | |
| void | _publish_vehicle_pose () |
| publishes the localization ('vehicle_pose') to the topic vehicle_pose | |
| void | _publish_map () |
| publishes the map ('track_map') to the topic track_map | |
| void | _publish_associations () |
| Publishes the visualization of the associations by data association. | |
| void | _publish_covariance () |
| publishes the covariance of the state | |
| void | _publish_lap_counter () |
| publishes the lap counter | |
| bool | _is_mission_finished () |
| Checks if the mission is finished. | |
Protected Member Functions inherited from VENode | |
| void | publish_velocities () const |
Protected Attributes inherited from ControlNode | |
| bool | go_signal_ {false} |
| ControlParameters | params_ |
Protected Attributes inherited from SLAMNode | |
| rclcpp::CallbackGroup::SharedPtr | _callback_group_ |
| Callback group for subscriptions, used to control if they are concurrent or not. | |
| rclcpp::Subscription< custom_interfaces::msg::PerceptionOutput >::SharedPtr | _perception_subscription_ |
| rclcpp::Subscription< custom_interfaces::msg::Velocities >::SharedPtr | _velocities_subscription_ |
| rclcpp::Publisher< custom_interfaces::msg::Pose >::SharedPtr | _vehicle_pose_publisher_ |
| rclcpp::Publisher< custom_interfaces::msg::ConeArray >::SharedPtr | _map_publisher_ |
| rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr | _visualization_map_publisher_ |
| rclcpp::Publisher< visualization_msgs::msg::Marker >::SharedPtr | _position_visualization_publisher_ |
| Publishes the vehicle position. | |
| rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr | _trajectory_visualization_publisher_ |
| Publishes full vehicle trajectory. | |
| rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr | _associations_visualization_publisher_ |
| rclcpp::Publisher< std_msgs::msg::Float64MultiArray >::SharedPtr | _execution_time_publisher_ |
| rclcpp::Publisher< std_msgs::msg::Float64MultiArray >::SharedPtr | _covariance_publisher_ |
| Publishes the covariance of the pose. | |
| rclcpp::Publisher< std_msgs::msg::Float64 >::SharedPtr | _lap_counter_publisher_ |
| rclcpp::Publisher< std_msgs::msg::Float64 >::SharedPtr | _perception_delta_publisher_ |
| rclcpp::Time | _last_perception_message_time_ = rclcpp::Time(0) |
| rclcpp::CallbackGroup::SharedPtr | _parallel_callback_group_ |
| std::shared_mutex | mutex_ |
| std::shared_ptr< tf2_ros::TransformBroadcaster > | _tf_broadcaster_ |
| rclcpp::TimerBase::SharedPtr | _timer_ |
| timer | |
| std::shared_ptr< SLAMSolver > | _slam_solver_ |
| SLAM solver object. | |
| std::vector< common_lib::structures::Cone > | _perception_map_ |
| common_lib::structures::Velocities | _vehicle_state_velocities_ |
| std::vector< common_lib::structures::Cone > | _track_map_ |
| Eigen::VectorXi | _associations_ |
| Eigen::VectorXd | _observations_global_ |
| Global observations of the cones. | |
| Eigen::VectorXd | _map_coordinates_ |
| Coordinates of the cones in the map. | |
| common_lib::structures::Pose | _vehicle_pose_ |
| std::shared_ptr< std::vector< double > > | _execution_times_ |
| common_lib::competition_logic::Mission | _mission_ = common_lib::competition_logic::Mission::NONE |
| bool | _go_ = true |
| std::string | _adapter_name_ |
| flag to start the mission | |
| std::string | _vehicle_frame_id_ |
| Frame id of the vehicle for the transform. | |
| SLAMParameters | _params_ |
Protected Attributes inherited from VENode | |
| VEParameters | _parameters_ |
| std::shared_ptr< VelocityEstimator > | _velocity_estimator_ |
| rclcpp::Publisher< custom_interfaces::msg::Velocities >::SharedPtr | _velocities_pub_ |
Adapter for interfacing with the real vehicle hardware.
Adapter class for the vehicle.
Adapter class to interface with the vehicle for SLAM.
Works on a publish-subscribe model. Publishes commands to the vehicle in the form of ControlCommand messages and subscribes to OperationalStatus messages to receive a go signal.
This class subscribes to the necessary topics from the vehicle and adapts the data for use in the SLAM node
This class subscribes to the IMU, wheel speed sensors, and steering angle topics published by the vehicle. It then passes the data to the velocity estimator for processing.
Definition at line 15 of file vehicle.hpp.
|
private |
Policy for synchronizing IMU data.
Definition at line 35 of file vehicle.hpp.
|
private |
Policy for synchronizing wheel speeds.
Definition at line 34 of file vehicle_adapter.hpp.
|
private |
Policy for synchronizing wheel speeds and steering angle.
Definition at line 41 of file vehicle.hpp.
|
private |
Policy for synchronizing Xsens IMU data.
Definition at line 38 of file vehicle.hpp.
|
private |
Policy for synchronizing Xsens IMU data.
Definition at line 31 of file vehicle_adapter.hpp.
|
explicit |
Definition at line 3 of file vehicle.cpp.
|
explicit |
|
explicit |
| VehicleAdapter::VehicleAdapter | ( | const SLAMParameters & | params | ) |
Constructor of the vehicle adapter node.
Definition at line 13 of file vehicle.cpp.

|
explicit |
| void VehicleAdapter::_lidar_odometry_subscription_callback | ( | const nav_msgs::msg::Odometry & | msg | ) |
LiDAR odometry subscription callback.
| msg | Pose estimated by the LiDAR odometry |
Definition at line 77 of file vehicle.cpp.


|
finalvirtual |
Function that sends the finish signal to the respective node.
Implements Adapter.
Definition at line 76 of file vehicle.cpp.
|
overridevirtual |
Function that sends the finish signal to the respective node.
Implements Adapter.
|
overridevirtual |
Sends the finished signal to the vehicle control.
Implements Adapter.
| void VehicleAdapter::go_signal_callback | ( | const custom_interfaces::msg::OperationalStatus | msg | ) |
Definition at line 25 of file vehicle.cpp.
| void VehicleAdapter::imu_callback | ( | const geometry_msgs::msg::Vector3Stamped::SharedPtr & | free_acceleration_msg, |
| const geometry_msgs::msg::Vector3Stamped::SharedPtr & | angular_velocity_msg | ||
| ) |
IMU subscription callback, which receives both free acceleration and angular velocity through a synchronizer message filter.
Definition at line 47 of file vehicle_adapter.cpp.


|
private |
IMU subscriptio callback, which receives both roll and yaw acceleration data separately and synchronizes them.
| roll_accx_data | roll and acceleration in x axis data |
| yaw_accy_data | yaw and acceleration in y axis data |
Definition at line 55 of file vehicle.cpp.

| void VehicleAdapter::mission_state_callback | ( | const custom_interfaces::msg::OperationalStatus & | msg | ) |
|
overridevirtual |
Adapters override this function to publish control commands in their environment.
| cmd | Control command to be published |
Implements ControlNode.
Definition at line 13 of file vehicle.cpp.
| void VehicleAdapter::resolver_callback | ( | custom_interfaces::msg::WheelRPM | msg | ) |
Callback for the subscription of the resolver which measures the motor RPM.
Definition at line 66 of file vehicle_adapter.cpp.


| void VehicleAdapter::steering_angle_callback | ( | const custom_interfaces::msg::SteeringAngle | msg | ) |
Callback for the subscription of the steering angle sensor.
Definition at line 42 of file vehicle_adapter.cpp.


|
private |
Wheel speed subscription callback, which receives both wheel speeds and steering angle through a synchronizer message filter.
Definition at line 44 of file vehicle.cpp.

| void VehicleAdapter::wss_callback | ( | const custom_interfaces::msg::WheelRPM & | rl_wheel_rpm_msg, |
| const custom_interfaces::msg::WheelRPM & | rr_wheel_rpm_msg | ||
| ) |
Wheel speed subscription callback, which receives both wheel speeds through a synchronizer message filter.
Definition at line 27 of file vehicle_adapter.cpp.


|
private |
Xsens IMU subscription callback, which receives both free acceleration and angular velocity data separately and synchronizes them to then call State Estimation node's IMU callback.
| free_acceleration_msg | acceleration without gravity in x, y and z axis |
| angular_velocity_msg | angular velocity around x, y and z axis |
Definition at line 65 of file vehicle.cpp.

|
private |
Definition at line 30 of file vehicle.hpp.
|
private |
Client for finished signal.
Definition at line 53 of file vehicle.hpp.
|
private |
Subscriber for fl wheel rpm.
Definition at line 25 of file vehicle_adapter.hpp.
|
private |
Subscriber for fr wheel rpm.
Definition at line 27 of file vehicle_adapter.hpp.
|
private |
Definition at line 29 of file vehicle.hpp.
|
private |
Definition at line 48 of file vehicle.hpp.
|
private |
Definition at line 20 of file vehicle.hpp.
|
private |
Subscriber for operational status.
Definition at line 27 of file vehicle.hpp.
|
private |
Definition at line 43 of file vehicle_adapter.hpp.
|
private |
Subscriber for rl wheel rpm.
Definition at line 21 of file vehicle.hpp.
|
private |
Definition at line 32 of file vehicle.hpp.
|
private |
Subscriber for rr wheel rpm.
Definition at line 23 of file vehicle.hpp.
|
private |
Definition at line 41 of file vehicle_adapter.hpp.
|
private |
Subscriber for steering angle.
Definition at line 25 of file vehicle.hpp.
|
private |
Synchronizer for wheel speeds and steering angle.
Definition at line 46 of file vehicle.hpp.
|
private |
Static transform broadcaster.
Definition at line 26 of file vehicle.hpp.
|
private |
Synchronizer for wheel speeds.
Definition at line 38 of file vehicle_adapter.hpp.
|
private |
Definition at line 50 of file vehicle.hpp.
|
private |
Definition at line 33 of file vehicle.hpp.
|
private |
Definition at line 46 of file vehicle_adapter.hpp.
|
private |
Definition at line 24 of file vehicle.hpp.
|
private |
Subscription for the go signal from the operational status, which activates the controller when true.
Definition at line 22 of file vehicle.hpp.
|
private |
Definition at line 45 of file vehicle_adapter.hpp.
|
private |
Definition at line 8 of file vehicle.hpp.
|
private |
Definition at line 47 of file vehicle_adapter.hpp.