pacsim_main.cpp File Reference

#include "rclcpp/rclcpp.hpp"
#include <sstream>
#include <chrono>
#include <condition_variable>
#include <limits>
#include <mutex>
#include <thread>
#include <vector>
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rosgraph_msgs/msg/clock.h"
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
#include "custom_interfaces/msg/vehicle_state_vector.hpp"
#include <sensor_msgs/msg/imu.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include "VehicleModel/VehicleModelBicycle.hpp"
#include "configParser.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
#include "track/trackLoader.hpp"
#include "logger.hpp"
#include "sensorModels/imuSensor.hpp"
#include "sensorModels/perceptionSensor.hpp"
#include "sensorModels/scalarValueSensor.hpp"
#include "sensorModels/wheelsSensor.hpp"
#include "VehicleModel/deadTime.hpp"
#include <rosgraph_msgs/msg/clock.hpp>
#include "ros2Helpers.hpp"
#include "pacsim/msg/perception_detections.hpp"
#include "pacsim/msg/stamped_scalar.hpp"
#include "pacsim/msg/wheels.hpp"
#include "pacsim/srv/clock_trigger_absolute.hpp"
#include "pacsim/srv/clock_trigger_relative.hpp"
#include "types.hpp"
#include "competitionLogic.hpp"
#include "std_srvs/srv/empty.hpp"
#include "reportWriter.hpp"
#include "sensorModels/gnssSensor.hpp"
#include "track/gripMap.hpp"

Include dependency graph for pacsim_main.cpp:

Go to the source code of this file.

Functions
void	publishStateEstimation ()
void	updateStateEstimation (const pacsim::msg::PerceptionDetections msg)
int	threadMainLoopFunc (std::shared_ptr< rclcpp::Node > node)
void	cbFuncLat (const pacsim::msg::StampedScalar &msg)
void	cbFuncLong (const pacsim::msg::Wheels &msg)
void	cbFuncTorquesInverterMin (const pacsim::msg::Wheels &msg)
void	cbFuncTorquesInverterMax (const pacsim::msg::Wheels &msg)
void	cbWheelspeeds (const pacsim::msg::Wheels &msg)
void	cbPowerGround (const pacsim::msg::StampedScalar &msg)
void	cbFinishSignal (const std::shared_ptr< std_srvs::srv::Empty::Request > request, std::shared_ptr< std_srvs::srv::Empty::Response > response)
void	cbClockTriggerAbsolute (const std::shared_ptr< pacsim::srv::ClockTriggerAbsolute::Request > request, std::shared_ptr< pacsim::srv::ClockTriggerAbsolute::Response > response)
void	cbClockTriggerRelative (const std::shared_ptr< pacsim::srv::ClockTriggerRelative::Request > request, std::shared_ptr< pacsim::srv::ClockTriggerRelative::Response > response)
void	getRos2Params (rclcpp::Node::SharedPtr &node)
void	initPerceptionSensors ()
void	initSensors ()
MainConfig	fillMainConfig (std::string path)
void	handleTf2StaticTransforms ()
int	main (int argc, char **argv)

Variables
std::shared_ptr< rclcpp::executors::SingleThreadedExecutor >	executor
std::shared_ptr< IVehicleModel >	model
std::string	mapFilePrefix
double	simTime = 0.0
rclcpp::Publisher< rosgraph_msgs::msg::Clock >::SharedPtr	clockPub
rclcpp::Publisher< geometry_msgs::msg::TwistWithCovarianceStamped >::SharedPtr	velocity_pub
rclcpp::Publisher< sensor_msgs::msg::Imu >::SharedPtr	imu_pub
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	mapVizPub
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	simulatedSeVizPub
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	stateEstimationPub
rclcpp::Publisher< pacsim::msg::StampedScalar >::SharedPtr	steeringFrontPub
rclcpp::Publisher< pacsim::msg::StampedScalar >::SharedPtr	steeringRearPub
rclcpp::Publisher< pacsim::msg::Wheels >::SharedPtr	wheelspeedPub
rclcpp::Publisher< pacsim::msg::Wheels >::SharedPtr	torquesPub
rclcpp::Publisher< pacsim::msg::StampedScalar >::SharedPtr	voltageSensorTSPub
rclcpp::Publisher< pacsim::msg::StampedScalar >::SharedPtr	currentSensorTSPub
rclcpp::Publisher< geometry_msgs::msg::TwistWithCovarianceStamped >::SharedPtr	pose_pub
rclcpp::Publisher< custom_interfaces::msg::VehicleStateVector >::SharedPtr	state_vector_pub
std::map< std::pair< double, double >, geometry_msgs::msg::Point >	detected_cones_map
rclcpp::Publisher< sensor_msgs::msg::JointState >::SharedPtr	jointStatePublisher
std::shared_ptr< tf2_ros::StaticTransformBroadcaster >	tf_static_broadcaster_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	perceptionVizPub
std::vector< rclcpp::Publisher< pacsim::msg::PerceptionDetections >::SharedPtr >	lms_pubs
std::map< std::shared_ptr< PerceptionSensor >, rclcpp::Publisher< pacsim::msg::PerceptionDetections >::SharedPtr >	perceptionSensorPublisherMap
std::map< std::shared_ptr< PerceptionSensor >, std::shared_ptr< LandmarksMarkerWrapper > >	perceptionSensorMarkersWrappersMap
std::map< std::shared_ptr< PerceptionSensor >, rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr >	perceptionSensorVizPublisherMap
std::map< std::shared_ptr< ImuSensor >, rclcpp::Publisher< sensor_msgs::msg::Imu >::SharedPtr >	imuPublisherMap
std::map< std::shared_ptr< GnssSensor >, rclcpp::Publisher< pacsim::msg::GNSS >::SharedPtr >	gnssPublisherMap
DeadTime< double >	deadTimeSteeringFront (0.0)
DeadTime< double >	deadTimeSteeringRear (0.0)
DeadTime< Wheels >	deadTimeThrottle (0.0)
DeadTime< Wheels >	deadTimeTorques (0.0)
DeadTime< Wheels >	deadTimeWspdSetpoints (0.0)
DeadTime< Wheels >	deadTimeMaxTorques (0.0)
DeadTime< Wheels >	deadTimeMinTorques (0.0)
DeadTime< double >	deadTimePowerGroundSetpoint (0.0)
std::mutex	mutexSimTime
std::string	trackName
std::string	grip_map_path
std::string	trackFrame
std::string	report_file_dir
std::string	main_config_path
std::string	perception_config_path
std::string	sensors_config_path
std::string	vehicle_model_config_path
std::string	discipline
std::vector< std::string >	jointNames = { "FL_steer", "FL_rotate", "FR_steer", "FR_rotate", "RR_rotate", "RL_rotate", "steering" }
double	realtimeRatio = 1.0
MainConfig	mainConfig
std::vector< std::shared_ptr< PerceptionSensor > >	perceptionSensors
std::vector< std::shared_ptr< ImuSensor > >	imus
std::vector< std::shared_ptr< GnssSensor > >	gnssSensors
std::shared_ptr< CompetitionLogic >	cl
std::shared_ptr< ImuSensor >	imuSensor
std::shared_ptr< ScalarValueSensor >	steeringSensorFront
std::shared_ptr< ScalarValueSensor >	steeringSensorRear
std::shared_ptr< WheelsSensor >	wheelspeedSensor
std::shared_ptr< WheelsSensor >	torquesSensor
std::shared_ptr< ScalarValueSensor >	currentSensorTS
std::shared_ptr< ScalarValueSensor >	voltageSensorTS
std::shared_ptr< Logger >	logger
std::condition_variable	cvClockTrigger
double	clockStopTime = std::numeric_limits<double>::max()

Function Documentation

cbClockTriggerAbsolute()

void cbClockTriggerAbsolute	(	const std::shared_ptr< pacsim::srv::ClockTriggerAbsolute::Request >	request,
		std::shared_ptr< pacsim::srv::ClockTriggerAbsolute::Response >	response
	)

Definition at line 501 of file pacsim_main.cpp.

Here is the caller graph for this function:

cbClockTriggerRelative()

void cbClockTriggerRelative	(	const std::shared_ptr< pacsim::srv::ClockTriggerRelative::Request >	request,
		std::shared_ptr< pacsim::srv::ClockTriggerRelative::Response >	response
	)

Definition at line 510 of file pacsim_main.cpp.

Here is the caller graph for this function:

cbFinishSignal()

void cbFinishSignal	(	const std::shared_ptr< std_srvs::srv::Empty::Request >	request,
		std::shared_ptr< std_srvs::srv::Empty::Response >	response
	)

Definition at line 495 of file pacsim_main.cpp.

Here is the caller graph for this function:

cbFuncLat()

void cbFuncLat

(

const pacsim::msg::StampedScalar &

msg

)

Definition at line 441 of file pacsim_main.cpp.

Here is the caller graph for this function:

cbFuncLong()

void cbFuncLong

(

const pacsim::msg::Wheels &

msg

)

Definition at line 448 of file pacsim_main.cpp.

Here is the caller graph for this function:

cbFuncTorquesInverterMax()

void cbFuncTorquesInverterMax

(

const pacsim::msg::Wheels &

msg

)

Definition at line 471 of file pacsim_main.cpp.

Here is the caller graph for this function:

cbFuncTorquesInverterMin()

void cbFuncTorquesInverterMin

(

const pacsim::msg::Wheels &

msg

)

Definition at line 460 of file pacsim_main.cpp.

Here is the caller graph for this function:

cbPowerGround()

void cbPowerGround

(

const pacsim::msg::StampedScalar &

msg

)

Definition at line 489 of file pacsim_main.cpp.

Here is the caller graph for this function:

cbWheelspeeds()

void cbWheelspeeds

(

const pacsim::msg::Wheels &

msg

)

Definition at line 482 of file pacsim_main.cpp.

Here is the caller graph for this function:

fillMainConfig()

MainConfig fillMainConfig

(

std::string

path

)

Definition at line 694 of file pacsim_main.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

getRos2Params()

void getRos2Params

(

rclcpp::Node::SharedPtr &

node

)

Definition at line 518 of file pacsim_main.cpp.

Here is the caller graph for this function:

handleTf2StaticTransforms()

void handleTf2StaticTransforms

(

)

Definition at line 720 of file pacsim_main.cpp.

Here is the caller graph for this function:

initPerceptionSensors()

void initPerceptionSensors

(

)

Definition at line 541 of file pacsim_main.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

initSensors()

void initSensors

(

)

Definition at line 648 of file pacsim_main.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 750 of file pacsim_main.cpp.

Here is the call graph for this function:

publishStateEstimation()

void publishStateEstimation

(

)

Definition at line 614 of file pacsim_main.cpp.

Here is the caller graph for this function:

threadMainLoopFunc()

int threadMainLoopFunc

(

std::shared_ptr< rclcpp::Node >

node

)

Definition at line 138 of file pacsim_main.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

updateStateEstimation()

void updateStateEstimation

(

const pacsim::msg::PerceptionDetections

msg

)

Definition at line 556 of file pacsim_main.cpp.

Here is the caller graph for this function:

Variable Documentation

cl

std::shared_ptr<CompetitionLogic> cl

Definition at line 123 of file pacsim_main.cpp.

clockPub

rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clockPub

Definition at line 64 of file pacsim_main.cpp.

clockStopTime

double clockStopTime = std::numeric_limits<double>::max()

Definition at line 136 of file pacsim_main.cpp.

currentSensorTS

std::shared_ptr<ScalarValueSensor> currentSensorTS

Definition at line 130 of file pacsim_main.cpp.

currentSensorTSPub

rclcpp::Publisher<pacsim::msg::StampedScalar>::SharedPtr currentSensorTSPub

Definition at line 75 of file pacsim_main.cpp.

cvClockTrigger

std::condition_variable cvClockTrigger

Definition at line 135 of file pacsim_main.cpp.

deadTimeMaxTorques

DeadTime< Wheels > deadTimeMaxTorques(0.0)

(

0.

0

)

deadTimeMinTorques

DeadTime< Wheels > deadTimeMinTorques(0.0)

(

0.

0

)

deadTimePowerGroundSetpoint

DeadTime< double > deadTimePowerGroundSetpoint(0.0)

(

0.

0

)

deadTimeSteeringFront

DeadTime< double > deadTimeSteeringFront(0.0)

(

0.

0

)

deadTimeSteeringRear

DeadTime< double > deadTimeSteeringRear(0.0)

(

0.

0

)

deadTimeThrottle

DeadTime< Wheels > deadTimeThrottle(0.0)

(

0.

0

)

deadTimeTorques

DeadTime< Wheels > deadTimeTorques(0.0)

(

0.

0

)

deadTimeWspdSetpoints

DeadTime< Wheels > deadTimeWspdSetpoints(0.0)

(

0.

0

)

detected_cones_map

std::map<std::pair<double, double>, geometry_msgs::msg::Point> detected_cones_map

Definition at line 78 of file pacsim_main.cpp.

discipline

std::string discipline

Definition at line 115 of file pacsim_main.cpp.

executor

std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor

Definition at line 60 of file pacsim_main.cpp.

gnssPublisherMap

std::map<std::shared_ptr<GnssSensor>, rclcpp::Publisher<pacsim::msg::GNSS>::SharedPtr> gnssPublisherMap

Definition at line 94 of file pacsim_main.cpp.

gnssSensors

std::vector<std::shared_ptr<GnssSensor> > gnssSensors

Definition at line 122 of file pacsim_main.cpp.

grip_map_path

std::string grip_map_path

Definition at line 108 of file pacsim_main.cpp.

imu_pub

rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub

Definition at line 66 of file pacsim_main.cpp.

imuPublisherMap

std::map<std::shared_ptr<ImuSensor>, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> imuPublisherMap

Definition at line 93 of file pacsim_main.cpp.

imus

std::vector<std::shared_ptr<ImuSensor> > imus

Definition at line 121 of file pacsim_main.cpp.

imuSensor

std::shared_ptr<ImuSensor> imuSensor

Definition at line 125 of file pacsim_main.cpp.

jointNames

std::vector<std::string> jointNames = { "FL_steer", "FL_rotate", "FR_steer", "FR_rotate", "RR_rotate", "RL_rotate", "steering" }

Definition at line 116 of file pacsim_main.cpp.

jointStatePublisher

rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr jointStatePublisher

Definition at line 82 of file pacsim_main.cpp.

lms_pubs

std::vector<rclcpp::Publisher<pacsim::msg::PerceptionDetections>::SharedPtr> lms_pubs

Definition at line 87 of file pacsim_main.cpp.

logger

std::shared_ptr<Logger> logger

Definition at line 133 of file pacsim_main.cpp.

main_config_path

std::string main_config_path

Definition at line 111 of file pacsim_main.cpp.

mainConfig

MainConfig mainConfig

Definition at line 119 of file pacsim_main.cpp.

mapFilePrefix

std::string mapFilePrefix

Definition at line 62 of file pacsim_main.cpp.

mapVizPub

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr mapVizPub

Definition at line 67 of file pacsim_main.cpp.

model

std::shared_ptr<IVehicleModel> model

Definition at line 61 of file pacsim_main.cpp.

mutexSimTime

std::mutex mutexSimTime

Definition at line 105 of file pacsim_main.cpp.

perception_config_path

std::string perception_config_path

Definition at line 112 of file pacsim_main.cpp.

perceptionSensorMarkersWrappersMap

std::map<std::shared_ptr<PerceptionSensor>, std::shared_ptr<LandmarksMarkerWrapper> > perceptionSensorMarkersWrappersMap

Definition at line 90 of file pacsim_main.cpp.

perceptionSensorPublisherMap

std::map<std::shared_ptr<PerceptionSensor>, rclcpp::Publisher<pacsim::msg::PerceptionDetections>::SharedPtr> perceptionSensorPublisherMap

Definition at line 89 of file pacsim_main.cpp.

perceptionSensors

std::vector<std::shared_ptr<PerceptionSensor> > perceptionSensors

Definition at line 120 of file pacsim_main.cpp.

perceptionSensorVizPublisherMap

std::map<std::shared_ptr<PerceptionSensor>, rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr> perceptionSensorVizPublisherMap

Definition at line 92 of file pacsim_main.cpp.

perceptionVizPub

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr perceptionVizPub

Definition at line 86 of file pacsim_main.cpp.

pose_pub

rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr pose_pub

Definition at line 76 of file pacsim_main.cpp.

realtimeRatio

double realtimeRatio = 1.0

Definition at line 118 of file pacsim_main.cpp.

report_file_dir

std::string report_file_dir

Definition at line 110 of file pacsim_main.cpp.

sensors_config_path

std::string sensors_config_path

Definition at line 113 of file pacsim_main.cpp.

simTime

double simTime = 0.0

Definition at line 63 of file pacsim_main.cpp.

simulatedSeVizPub

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr simulatedSeVizPub

Definition at line 68 of file pacsim_main.cpp.

state_vector_pub

rclcpp::Publisher<custom_interfaces::msg::VehicleStateVector>::SharedPtr state_vector_pub

Definition at line 77 of file pacsim_main.cpp.

stateEstimationPub

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr stateEstimationPub

Definition at line 69 of file pacsim_main.cpp.

steeringFrontPub

rclcpp::Publisher<pacsim::msg::StampedScalar>::SharedPtr steeringFrontPub

Definition at line 70 of file pacsim_main.cpp.

steeringRearPub

rclcpp::Publisher<pacsim::msg::StampedScalar>::SharedPtr steeringRearPub

Definition at line 71 of file pacsim_main.cpp.

steeringSensorFront

std::shared_ptr<ScalarValueSensor> steeringSensorFront

Definition at line 126 of file pacsim_main.cpp.

steeringSensorRear

std::shared_ptr<ScalarValueSensor> steeringSensorRear

Definition at line 127 of file pacsim_main.cpp.

tf_static_broadcaster_

std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_

Definition at line 84 of file pacsim_main.cpp.

torquesPub

rclcpp::Publisher<pacsim::msg::Wheels>::SharedPtr torquesPub

Definition at line 73 of file pacsim_main.cpp.

torquesSensor

std::shared_ptr<WheelsSensor> torquesSensor

Definition at line 129 of file pacsim_main.cpp.

trackFrame

std::string trackFrame

Definition at line 109 of file pacsim_main.cpp.

trackName

std::string trackName

Definition at line 107 of file pacsim_main.cpp.

vehicle_model_config_path

std::string vehicle_model_config_path

Definition at line 114 of file pacsim_main.cpp.

velocity_pub

rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr velocity_pub

Definition at line 65 of file pacsim_main.cpp.

voltageSensorTS

std::shared_ptr<ScalarValueSensor> voltageSensorTS

Definition at line 131 of file pacsim_main.cpp.

voltageSensorTSPub

rclcpp::Publisher<pacsim::msg::StampedScalar>::SharedPtr voltageSensorTSPub

Definition at line 74 of file pacsim_main.cpp.

wheelspeedPub

rclcpp::Publisher<pacsim::msg::Wheels>::SharedPtr wheelspeedPub

Definition at line 72 of file pacsim_main.cpp.

wheelspeedSensor

std::shared_ptr<WheelsSensor> wheelspeedSensor

Definition at line 128 of file pacsim_main.cpp.