Skip to content

Commit be17691

Browse files
mateusz-lichotamateusz-lichota
committed
plublish simulation time to /clock
1 parent 7c51418 commit be17691

3 files changed

Lines changed: 32 additions & 101 deletions

File tree

‎ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@ STRICT_MODE_OFF //todo what does this do?
2525
#include <geometry_msgs/msg/transform_stamped.hpp>
2626
#include <geometry_msgs/msg/twist_stamped.hpp>
2727
#include <geometry_msgs/msg/twist.hpp>
28+
#include <rosgraph_msgs/msg/clock.hpp>
2829
#include <iostream>
2930
#include <math.h>
3031
#include <math_common.h>
@@ -131,6 +132,7 @@ class AirsimROSWrapper
131132
void statistics_timer_cb();
132133
void go_signal_timer_cb();
133134
void extra_info_cb();
135+
void clock_timer_cb();
134136

135137
/// ROS subscriber callbacks
136138
void finished_signal_cb(const fs_msgs::msg::FinishedSignal& msg);
@@ -206,6 +208,7 @@ class AirsimROSWrapper
206208
rclcpp::TimerBase::SharedPtr go_signal_timer_;
207209
rclcpp::TimerBase::SharedPtr statictf_timer_;
208210
rclcpp::TimerBase::SharedPtr extra_info_timer_;
211+
rclcpp::TimerBase::SharedPtr clock_timer_;
209212

210213
std::vector<std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>>> lidar_pub_vec_;
211214

@@ -218,6 +221,7 @@ class AirsimROSWrapper
218221
std::shared_ptr<rclcpp::Publisher<fs_msgs::msg::Track>> track_pub;
219222
std::shared_ptr<rclcpp::Publisher<fs_msgs::msg::GoSignal>> go_signal_pub_;
220223
std::shared_ptr<rclcpp::Publisher<fs_msgs::msg::ExtraInfo>> extra_info_pub;
224+
std::shared_ptr<rclcpp::Publisher<rosgraph_msgs::msg::Clock>> clock_pub;
221225

222226
/// ROS subscribers
223227
std::shared_ptr<rclcpp::Subscription<std::remove_cv_t<std::remove_reference_t<const fs_msgs::msg::ControlCommand &>>, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<std::remove_cv_t<std::remove_reference_t<const fs_msgs::msg::ControlCommand &>>, std::allocator<void>>>> control_cmd_sub;

‎ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -139,6 +139,7 @@ void AirsimROSWrapper::initialize_ros()
139139
odom_update_timer_ = nh_->create_wall_timer(dseconds{update_odom_every_n_sec}, std::bind(&AirsimROSWrapper::odom_cb, this));
140140
extra_info_timer_ = nh_->create_wall_timer(dseconds{1}, std::bind(&AirsimROSWrapper::extra_info_cb, this));
141141
}
142+
clock_timer_ = nh_->create_wall_timer(dseconds{0.01}, std::bind(&AirsimROSWrapper::clock_timer_cb, this));
142143

143144
gps_update_timer_ = nh_->create_wall_timer(dseconds{update_gps_every_n_sec}, std::bind(&AirsimROSWrapper::gps_timer_cb, this));
144145
imu_update_timer_ = nh_->create_wall_timer(dseconds{update_imu_every_n_sec}, std::bind(&AirsimROSWrapper::imu_timer_cb, this));
@@ -182,6 +183,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
182183
set_nans_to_zeros_in_pose(*vehicle_setting);
183184

184185
vehicle_name = curr_vehicle_name;
186+
clock_pub = nh_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 10);
185187
global_gps_pub = nh_->create_publisher<sensor_msgs::msg::NavSatFix>("gps", 10);
186188
imu_pub = nh_->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
187189
gss_pub = nh_->create_publisher<geometry_msgs::msg::TwistStamped>("gss", 10);
@@ -832,6 +834,31 @@ void AirsimROSWrapper::finished_signal_cb(const fs_msgs::msg::FinishedSignal& ms
832834
curl_global_cleanup();
833835
}
834836

837+
void AirsimROSWrapper::clock_timer_cb(){
838+
// I'm really sorry for this code, but airsim_client_ doesn't seem to expose
839+
// a method to get just the time, so it's necessary to request data from some
840+
// sensor and use its timestamp to get the current simulation time.
841+
try{
842+
struct msr::airlib::GSSSimple::Output gss_data;
843+
{
844+
ros_bridge::Timer timer(&getGSSStatistics);
845+
std::unique_lock<std::recursive_mutex> lck(car_control_mutex_);
846+
gss_data = airsim_client_.getGroundSpeedSensorData(vehicle_name);
847+
lck.unlock();
848+
849+
rosgraph_msgs::msg::Clock clock_msg;
850+
clock_msg.clock = make_ts(gss_data.time_stamp);
851+
clock_pub->publish(clock_msg);
852+
}
853+
} catch (rpc::rpc_error& e)
854+
{
855+
std::string msg = e.get_error().as<std::string>();
856+
std::cout << "Exception raised by the API, something went wrong while retrieving gss data for publishing simulation clock.\n"
857+
<< msg << std::endl;
858+
}
859+
860+
}
861+
835862
void AirsimROSWrapper::extra_info_cb(){
836863
CarApiBase::RefereeState state;
837864
try{

‎settings.json‎

Lines changed: 1 addition & 101 deletions
Original file line numberDiff line numberDiff line change
@@ -1,101 +1 @@
1-
{
2-
"SeeDocsAt": "https://FS-Driverless.github.io/Formula-Student-Driverless-Simulator/",
3-
"SettingsVersion": 1.2,
4-
"ViewMode": "SpringArmChase",
5-
"ClockSpeed": 1.0,
6-
"SpectatorServerPassword": "password",
7-
"PawnPaths": {
8-
"DefaultCar": { "PawnBP": "Class'/AirSim/VehicleAdv/Cars/TechnionCar/TechnionCarPawn.TechnionCarPawn_C'" }
9-
},
10-
"Vehicles": {
11-
"FSCar": {
12-
"DefaultVehicleState": "",
13-
"EnableCollisionPassthrogh": false,
14-
"EnableCollisions": true,
15-
"AllowAPIAlways": true,
16-
"RC":{
17-
"RemoteControlID": -1
18-
},
19-
"Sensors": {
20-
"Imu" : {
21-
"SensorType": 2,
22-
"Enabled": true
23-
},
24-
"Gps" : {
25-
"SensorType": 3,
26-
"Enabled": true
27-
},
28-
"Lidar1": {
29-
"SensorType": 6,
30-
"Enabled": true,
31-
"X": 0.45, "Y": 0, "Z": 0.55,
32-
"Roll": 0, "Pitch": 0, "Yaw" : 0,
33-
"NumberOfLasers": 4,
34-
"PointsPerScan": 4096,
35-
"VerticalFOVUpper": 5,
36-
"VerticalFOVLower": -5,
37-
"HorizontalFOVStart": -90,
38-
"HorizontalFOVEnd": 90,
39-
"RotationsPerSecond": 10,
40-
"DrawDebugPoints": false
41-
},
42-
"Lidar2": {
43-
"SensorType": 6,
44-
"Enabled": true,
45-
"X": 1.20, "Y": 0, "Z": 0.2,
46-
"Roll": 0, "Pitch": 0, "Yaw" : 0,
47-
"NumberOfLasers": 4,
48-
"PointsPerScan": 4096,
49-
"VerticalFOVUpper": 5,
50-
"VerticalFOVLower": -5,
51-
"HorizontalFOVStart": -90,
52-
"HorizontalFOVEnd": 90,
53-
"RotationsPerSecond": 5,
54-
"DrawDebugPoints": false
55-
},
56-
"GSS" : {
57-
"SensorType": 7,
58-
"Enabled": true
59-
}
60-
},
61-
"Cameras": {
62-
"cam1": {
63-
"CaptureSettings": [
64-
{
65-
"ImageType": 0,
66-
"Width": 785,
67-
"Height": 785,
68-
"FOV_Degrees": 90
69-
}
70-
],
71-
"X": -0.3,
72-
"Y": -0.16,
73-
"Z": 0.8,
74-
"Pitch": 0.0,
75-
"Roll": 0.0,
76-
"Yaw": 0
77-
},
78-
"cam2": {
79-
"CaptureSettings": [
80-
{
81-
"ImageType": 0,
82-
"Width": 785,
83-
"Height": 785,
84-
"FOV_Degrees": 90
85-
}
86-
],
87-
"X": -0.3,
88-
"Y": 0.16,
89-
"Z": 0.8,
90-
"Pitch": 0.0,
91-
"Roll": 0.0,
92-
"Yaw": 0.0
93-
}
94-
},
95-
"X": 2, "Y": 0, "Z": 0,
96-
"Pitch": 0, "Roll": 0, "Yaw": 0
97-
}
98-
},
99-
"SubWindows": [
100-
]
101-
}
1+
{"SeeDocsAt": "https://FS-Driverless.github.io/Formula-Student-Driverless-Simulator/", "SettingsVersion": 1.2, "ViewMode": "SpringArmChase", "ClockSpeed": 1.0, "SpectatorServerPassword": "password", "PawnPaths": {"DefaultCar": {"PawnBP": "Class'/AirSim/VehicleAdv/Cars/TechnionCar/TechnionCarPawn.TechnionCarPawn_C'"}}, "Vehicles": {"FSCar": {"DefaultVehicleState": "", "EnableCollisionPassthrogh": false, "EnableCollisions": true, "AllowAPIAlways": true, "RC": {"RemoteControlID": -1}, "Sensors": {"Imu": {"SensorType": 2, "Enabled": true}, "Gps": {"SensorType": 3, "Enabled": true}, "Lidar1": {"SensorType": 6, "Enabled": true, "X": 2.35, "Y": 0.0, "Z": 0.2, "Roll": 0, "Pitch": 0, "Yaw": 0, "NumberOfLasers": 1, "PointsPerScan": 1000, "VerticalFOVUpper": 1, "VerticalFOVLower": -1, "HorizontalFOVStart": -120.32113697747289, "HorizontalFOVEnd": 120.32113697747289, "RotationsPerSecond": 10, "DrawDebugPoints": false}, "GSS": {"SensorType": 7, "Enabled": true}}, "Cameras": {"cam1": {"CaptureSettings": [{"ImageType": 0, "Width": 785, "Height": 785, "FOV_Degrees": 90}], "X": -0.3, "Y": -0.16, "Z": 0.8, "Pitch": 0.0, "Roll": 0.0, "Yaw": 0}, "cam2": {"CaptureSettings": [{"ImageType": 0, "Width": 785, "Height": 785, "FOV_Degrees": 90}], "X": -0.3, "Y": 0.16, "Z": 0.8, "Pitch": 0.0, "Roll": 0.0, "Yaw": 0.0}}, "X": 2, "Y": 0, "Z": 0, "Pitch": 0, "Roll": 0, "Yaw": 0}}, "SubWindows": []}

0 commit comments

Comments
 (0)