Skip to content

Commit 779d29e

Browse files
dakejahldakejahl
committed
fix 0 timestamp issue by waiting for clock callback before subscribing to other topics. Refactor to cleanup topic subscriptions
1 parent 3eddbce commit 779d29e

File tree

2 files changed

+193
-70
lines changed

2 files changed

+193
-70
lines changed

‎src/modules/simulation/gz_bridge/GZBridge.cpp

Lines changed: 178 additions & 70 deletions
Original file line numberDiff line numberDiff line change
@@ -43,8 +43,6 @@
4343
#include <iostream>
4444
#include <string>
4545

46-
static float generate_wgn();
47-
4846
GZBridge::GZBridge(const std::string &world, const std::string &model_name) :
4947
ModuleParams(nullptr),
5048
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
@@ -63,119 +61,256 @@ GZBridge::~GZBridge()
6361

6462
int GZBridge::init()
6563
{
66-
// clock
64+
// REQUIRED:
65+
if (!subscribeClock(true)) {
66+
return PX4_ERROR;
67+
}
68+
69+
// We must wait for clock before subscribing to other topics. This is because
70+
// if we publish a 0 timestamp it screws up the EKF.
71+
while (1) {
72+
px4_usleep(25000);
73+
if (_realtime_clock_set) {
74+
px4_usleep(25000);
75+
break;
76+
}
77+
}
78+
79+
if (!subscribePoseInfo(true)) {
80+
return PX4_ERROR;
81+
}
82+
83+
if (!subscribeImu(true)) {
84+
return PX4_ERROR;
85+
}
86+
87+
if (!subscribeMag(true)) {
88+
return PX4_ERROR;
89+
}
90+
91+
// OPTIONAL:
92+
if (!subscribeNavsat(false)) {
93+
return PX4_ERROR;
94+
}
95+
96+
if (!subscribeAirPressure(false)) {
97+
return PX4_ERROR;
98+
}
99+
100+
if (!subscribeDistanceSensor(false)) {
101+
return PX4_ERROR;
102+
}
103+
104+
if (!subscribeAirspeed(false)) {
105+
return PX4_ERROR;
106+
}
107+
108+
if (!subscribeOpticalFlow(false)) {
109+
return PX4_ERROR;
110+
}
111+
112+
if (!subscribeOdometry(false)) {
113+
return PX4_ERROR;
114+
}
115+
116+
if (!subscribeLaserScan(false)) {
117+
return PX4_ERROR;
118+
}
119+
120+
// ESC mixing interface
121+
if (!_mixing_interface_esc.init(_model_name)) {
122+
PX4_ERR("failed to init ESC output");
123+
return PX4_ERROR;
124+
}
125+
126+
// Servo mixing interface
127+
if (!_mixing_interface_servo.init(_model_name)) {
128+
PX4_ERR("failed to init servo output");
129+
return PX4_ERROR;
130+
}
131+
132+
// Wheel mixing interface
133+
if (!_mixing_interface_wheel.init(_model_name)) {
134+
PX4_ERR("failed to init motor output");
135+
return PX4_ERROR;
136+
}
137+
138+
// Gimbal mixing interface
139+
if (!_gimbal.init(_world_name, _model_name)) {
140+
PX4_ERR("failed to init gimbal");
141+
return PX4_ERROR;
142+
}
143+
144+
ScheduleNow();
145+
return OK;
146+
}
147+
148+
void GZBridge::Run()
149+
{
150+
if (should_exit()) {
151+
ScheduleClear();
152+
153+
_mixing_interface_esc.stop();
154+
_mixing_interface_servo.stop();
155+
_mixing_interface_wheel.stop();
156+
_gimbal.stop();
157+
158+
exit_and_cleanup();
159+
return;
160+
}
161+
162+
if (_parameter_update_sub.updated()) {
163+
parameter_update_s pupdate;
164+
_parameter_update_sub.copy(&pupdate);
165+
166+
updateParams();
167+
168+
_mixing_interface_esc.updateParams();
169+
_mixing_interface_servo.updateParams();
170+
_mixing_interface_wheel.updateParams();
171+
_gimbal.updateParams();
172+
}
173+
174+
ScheduleDelayed(10_ms);
175+
}
176+
177+
bool GZBridge::subscribeClock(bool required)
178+
{
67179
std::string clock_topic = "/world/" + _world_name + "/clock";
68180

69181
if (!_node.Subscribe(clock_topic, &GZBridge::clockCallback, this)) {
70182
PX4_ERR("failed to subscribe to %s", clock_topic.c_str());
71-
return PX4_ERROR;
183+
return required ? false : true;
72184
}
73185

74-
// pose: /world/$WORLD/pose/info
186+
return true;
187+
}
188+
189+
bool GZBridge::subscribePoseInfo(bool required)
190+
{
75191
std::string world_pose_topic = "/world/" + _world_name + "/pose/info";
76192

77193
if (!_node.Subscribe(world_pose_topic, &GZBridge::poseInfoCallback, this)) {
78194
PX4_ERR("failed to subscribe to %s", world_pose_topic.c_str());
79-
return PX4_ERROR;
195+
return required ? false : true;
80196
}
81197

82-
// IMU: /world/$WORLD/model/$MODEL/link/base_link/sensor/imu_sensor/imu
198+
return true;
199+
}
200+
201+
bool GZBridge::subscribeImu(bool required)
202+
{
83203
std::string imu_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/imu_sensor/imu";
84204

85205
if (!_node.Subscribe(imu_topic, &GZBridge::imuCallback, this)) {
86206
PX4_ERR("failed to subscribe to %s", imu_topic.c_str());
87-
return PX4_ERROR;
207+
return required ? false : true;
88208
}
89209

90-
// mag: /world/$WORLD/model/$MODEL/link/base_link/sensor/magnetometer_sensor/magnetometer
210+
return true;
211+
}
212+
213+
bool GZBridge::subscribeMag(bool required)
214+
{
91215
std::string mag_topic = "/world/" + _world_name + "/model/" + _model_name +
92216
"/link/base_link/sensor/magnetometer_sensor/magnetometer";
93217

94218
if (!_node.Subscribe(mag_topic, &GZBridge::magnetometerCallback, this)) {
95219
PX4_ERR("failed to subscribe to %s", mag_topic.c_str());
96-
return PX4_ERROR;
220+
return required ? false : true;
97221
}
98222

223+
return true;
224+
}
225+
226+
bool GZBridge::subscribeOdometry(bool required)
227+
{
99228
// odom: /world/$WORLD/model/$MODEL/link/base_link/odometry_with_covariance
100229
std::string odometry_topic = "/model/" + _model_name + "/odometry_with_covariance";
101230

102231
if (!_node.Subscribe(odometry_topic, &GZBridge::odometryCallback, this)) {
103232
PX4_ERR("failed to subscribe to %s", odometry_topic.c_str());
104-
return PX4_ERROR;
233+
return required ? false : true;
105234
}
106235

107-
// Laser Scan: optional
236+
return true;
237+
}
238+
239+
bool GZBridge::subscribeLaserScan(bool required)
240+
{
108241
std::string laser_scan_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/link/sensor/lidar_2d_v2/scan";
109242

110243
if (!_node.Subscribe(laser_scan_topic, &GZBridge::laserScanCallback, this)) {
111244
PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str());
245+
return required ? false : true;
112246
}
113247

114-
// Distance Sensor(AFBRS50): optional
248+
return true;
249+
}
250+
251+
bool GZBridge::subscribeDistanceSensor(bool required)
252+
{
115253
std::string lidar_sensor = "/world/" + _world_name + "/model/" + _model_name +
116254
"/link/lidar_sensor_link/sensor/lidar/scan";
117255

118256
if (!_node.Subscribe(lidar_sensor, &GZBridge::laserScantoLidarSensorCallback, this)) {
119257
PX4_WARN("failed to subscribe to %s", lidar_sensor.c_str());
258+
return required ? false : true;
120259
}
121260

122-
// Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed
261+
return true;
262+
}
263+
264+
bool GZBridge::subscribeAirspeed(bool required)
265+
{
123266
std::string airspeed_topic = "/world/" + _world_name + "/model/" + _model_name +
124267
"/link/airspeed_link/sensor/air_speed/air_speed";
125268

126269
if (!_node.Subscribe(airspeed_topic, &GZBridge::airspeedCallback, this)) {
127270
PX4_ERR("failed to subscribe to %s", airspeed_topic.c_str());
128-
return PX4_ERROR;
271+
return required ? false : true;
129272
}
130273

131-
// Air pressure: /world/$WORLD/model/$MODEL/link/base_link/sensor/air_pressure_sensor/air_pressure
274+
return true;
275+
}
276+
277+
bool GZBridge::subscribeAirPressure(bool required)
278+
{
132279
std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name +
133280
"/link/base_link/sensor/air_pressure_sensor/air_pressure";
134281

135282
if (!_node.Subscribe(air_pressure_topic, &GZBridge::airPressureCallback, this)) {
136283
PX4_ERR("failed to subscribe to %s", air_pressure_topic.c_str());
137-
return PX4_ERROR;
284+
return required ? false : true;
138285
}
139286

140-
// GPS: /world/$WORLD/model/$MODEL/link/base_link/sensor/navsat_sensor/navsat
287+
return true;
288+
}
289+
290+
bool GZBridge::subscribeNavsat(bool required)
291+
{
141292
std::string nav_sat_topic = "/world/" + _world_name + "/model/" + _model_name +
142293
"/link/base_link/sensor/navsat_sensor/navsat";
143294

144295
if (!_node.Subscribe(nav_sat_topic, &GZBridge::navSatCallback, this)) {
145296
PX4_ERR("failed to subscribe to %s", nav_sat_topic.c_str());
146-
return PX4_ERROR;
297+
return required ? false : true;
147298
}
148299

300+
return true;
301+
}
302+
303+
bool GZBridge::subscribeOpticalFlow(bool required)
304+
{
149305
std::string flow_topic = "/world/" + _world_name + "/model/" + _model_name +
150306
"/link/flow_link/sensor/optical_flow/optical_flow";
151307

152308
if (!_node.Subscribe(flow_topic, &GZBridge::opticalFlowCallback, this)) {
153309
PX4_ERR("failed to subscribe to %s", flow_topic.c_str());
154-
return PX4_ERROR;
155-
}
156-
157-
if (!_mixing_interface_esc.init(_model_name)) {
158-
PX4_ERR("failed to init ESC output");
159-
return PX4_ERROR;
160-
}
161-
162-
if (!_mixing_interface_servo.init(_model_name)) {
163-
PX4_ERR("failed to init servo output");
164-
return PX4_ERROR;
310+
return required ? false : true;
165311
}
166312

167-
if (!_mixing_interface_wheel.init(_model_name)) {
168-
PX4_ERR("failed to init motor output");
169-
return PX4_ERROR;
170-
}
171-
172-
if (!_gimbal.init(_world_name, _model_name)) {
173-
PX4_ERR("failed to init gimbal");
174-
return PX4_ERROR;
175-
}
176-
177-
ScheduleNow();
178-
return OK;
313+
return true;
179314
}
180315

181316
void GZBridge::clockCallback(const gz::msgs::Clock &msg)
@@ -497,7 +632,7 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &msg)
497632
_visual_odometry_pub.publish(report);
498633
}
499634

500-
float generate_wgn()
635+
float GZBridge::generate_wgn()
501636
{
502637
// generate white Gaussian noise sample with std=1
503638

@@ -800,34 +935,7 @@ void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::m
800935
q_FRD_to_NED = q_ENU_to_NED * q_FLU_to_ENU * q_FLU_to_FRD.Inverse();
801936
}
802937

803-
void GZBridge::Run()
804-
{
805-
if (should_exit()) {
806-
ScheduleClear();
807938

808-
_mixing_interface_esc.stop();
809-
_mixing_interface_servo.stop();
810-
_mixing_interface_wheel.stop();
811-
_gimbal.stop();
812-
813-
exit_and_cleanup();
814-
return;
815-
}
816-
817-
if (_parameter_update_sub.updated()) {
818-
parameter_update_s pupdate;
819-
_parameter_update_sub.copy(&pupdate);
820-
821-
updateParams();
822-
823-
_mixing_interface_esc.updateParams();
824-
_mixing_interface_servo.updateParams();
825-
_mixing_interface_wheel.updateParams();
826-
_gimbal.updateParams();
827-
}
828-
829-
ScheduleDelayed(10_ms);
830-
}
831939

832940
int GZBridge::task_spawn(int argc, char *argv[])
833941
{

‎src/modules/simulation/gz_bridge/GZBridge.hpp

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -108,6 +108,19 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
108108

109109
void Run() override;
110110

111+
bool subscribeClock(bool required);
112+
bool subscribePoseInfo(bool required);
113+
bool subscribeImu(bool required);
114+
bool subscribeMag(bool required);
115+
bool subscribeOdometry(bool required);
116+
bool subscribeLaserScan(bool required);
117+
bool subscribeDistanceSensor(bool required);
118+
bool subscribeAirspeed(bool required);
119+
bool subscribeAirPressure(bool required);
120+
bool subscribeNavsat(bool required);
121+
bool subscribeOpticalFlow(bool required);
122+
123+
111124
void clockCallback(const gz::msgs::Clock &msg);
112125
void airspeedCallback(const gz::msgs::AirSpeed &msg);
113126
void airPressureCallback(const gz::msgs::FluidPressure &msg);
@@ -122,6 +135,8 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
122135

123136
static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU);
124137

138+
static float generate_wgn();
139+
125140
void addGpsNoise(double &latitude, double &longitude, double &altitude,
126141
float &vel_north, float &vel_east, float &vel_down);
127142

0 commit comments

Comments
 (0)