Skip to content

Commit 2fece23

Browse files
dakejahldakejahl
authored
gz small improvements (#24761)
* gz: print version number in init, remove gst plugin spam, rename function * fix 0 timestamp issue by waiting for clock callback before subscribing to other topics. Refactor to cleanup topic subscriptions * format * change gzerr to gzwarn
1 parent 9188480 commit 2fece23

File tree

4 files changed

+200
-82
lines changed

4 files changed

+200
-82
lines changed

‎ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,6 @@
11
#!/bin/sh
22
# shellcheck disable=SC2154
33

4-
echo "INFO [init] Gazebo simulator"
5-
64
# Enforce minimum gz version as Harmonic (gz-sim8)
75
MIN_GZ_VERSION="8.0.0"
86
GZ_SIM_VERSION=$(gz sim --versions 2>/dev/null | head -n 1 | tr -d ' ')
@@ -17,14 +15,16 @@ if [ "$(printf '%s\n' "$GZ_SIM_VERSION" "$MIN_GZ_VERSION" | sort -V | head -n1)"
1715
gz_command="gz"
1816
gz_sub_command="sim"
1917

18+
echo "INFO [init] Gazebo simulator $GZ_SIM_VERSION"
19+
2020
# Specify render engine if `GZ_SIM_RENDER_ENGINE` is set
2121
# (for example, if you want to use Ogre 1.x instead of Ogre 2.x):
2222
if [ -n "${PX4_GZ_SIM_RENDER_ENGINE}" ]; then
2323
echo "INFO [init] Setting Gazebo render engine to '${PX4_GZ_SIM_RENDER_ENGINE}'!"
2424
gz_sub_command="${gz_sub_command} --render-engine ${PX4_GZ_SIM_RENDER_ENGINE}"
2525
fi
2626
else
27-
echo "ERROR [init] Gazebo gz sim version is too old ($GZ_SIM_VERSION). Minimum required version is $MIN_GZ_VERSION"
27+
echo "ERROR [init] Gazebo version too hold ($GZ_SIM_VERSION). Minimum required version is $MIN_GZ_VERSION"
2828
exit 1
2929
fi
3030

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

Lines changed: 181 additions & 73 deletions
Original file line numberDiff line numberDiff line change
@@ -61,119 +61,257 @@ GZBridge::~GZBridge()
6161

6262
int GZBridge::init()
6363
{
64-
// 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+
74+
if (_realtime_clock_set) {
75+
px4_usleep(25000);
76+
break;
77+
}
78+
}
79+
80+
if (!subscribePoseInfo(true)) {
81+
return PX4_ERROR;
82+
}
83+
84+
if (!subscribeImu(true)) {
85+
return PX4_ERROR;
86+
}
87+
88+
if (!subscribeMag(true)) {
89+
return PX4_ERROR;
90+
}
91+
92+
// OPTIONAL:
93+
if (!subscribeNavsat(false)) {
94+
return PX4_ERROR;
95+
}
96+
97+
if (!subscribeAirPressure(false)) {
98+
return PX4_ERROR;
99+
}
100+
101+
if (!subscribeDistanceSensor(false)) {
102+
return PX4_ERROR;
103+
}
104+
105+
if (!subscribeAirspeed(false)) {
106+
return PX4_ERROR;
107+
}
108+
109+
if (!subscribeOpticalFlow(false)) {
110+
return PX4_ERROR;
111+
}
112+
113+
if (!subscribeOdometry(false)) {
114+
return PX4_ERROR;
115+
}
116+
117+
if (!subscribeLaserScan(false)) {
118+
return PX4_ERROR;
119+
}
120+
121+
// ESC mixing interface
122+
if (!_mixing_interface_esc.init(_model_name)) {
123+
PX4_ERR("failed to init ESC output");
124+
return PX4_ERROR;
125+
}
126+
127+
// Servo mixing interface
128+
if (!_mixing_interface_servo.init(_model_name)) {
129+
PX4_ERR("failed to init servo output");
130+
return PX4_ERROR;
131+
}
132+
133+
// Wheel mixing interface
134+
if (!_mixing_interface_wheel.init(_model_name)) {
135+
PX4_ERR("failed to init motor output");
136+
return PX4_ERROR;
137+
}
138+
139+
// Gimbal mixing interface
140+
if (!_gimbal.init(_world_name, _model_name)) {
141+
PX4_ERR("failed to init gimbal");
142+
return PX4_ERROR;
143+
}
144+
145+
ScheduleNow();
146+
return OK;
147+
}
148+
149+
void GZBridge::Run()
150+
{
151+
if (should_exit()) {
152+
ScheduleClear();
153+
154+
_mixing_interface_esc.stop();
155+
_mixing_interface_servo.stop();
156+
_mixing_interface_wheel.stop();
157+
_gimbal.stop();
158+
159+
exit_and_cleanup();
160+
return;
161+
}
162+
163+
if (_parameter_update_sub.updated()) {
164+
parameter_update_s pupdate;
165+
_parameter_update_sub.copy(&pupdate);
166+
167+
updateParams();
168+
169+
_mixing_interface_esc.updateParams();
170+
_mixing_interface_servo.updateParams();
171+
_mixing_interface_wheel.updateParams();
172+
_gimbal.updateParams();
173+
}
174+
175+
ScheduleDelayed(10_ms);
176+
}
177+
178+
bool GZBridge::subscribeClock(bool required)
179+
{
65180
std::string clock_topic = "/world/" + _world_name + "/clock";
66181

67182
if (!_node.Subscribe(clock_topic, &GZBridge::clockCallback, this)) {
68183
PX4_ERR("failed to subscribe to %s", clock_topic.c_str());
69-
return PX4_ERROR;
184+
return required ? false : true;
70185
}
71186

72-
// pose: /world/$WORLD/pose/info
187+
return true;
188+
}
189+
190+
bool GZBridge::subscribePoseInfo(bool required)
191+
{
73192
std::string world_pose_topic = "/world/" + _world_name + "/pose/info";
74193

75194
if (!_node.Subscribe(world_pose_topic, &GZBridge::poseInfoCallback, this)) {
76195
PX4_ERR("failed to subscribe to %s", world_pose_topic.c_str());
77-
return PX4_ERROR;
196+
return required ? false : true;
78197
}
79198

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

83206
if (!_node.Subscribe(imu_topic, &GZBridge::imuCallback, this)) {
84207
PX4_ERR("failed to subscribe to %s", imu_topic.c_str());
85-
return PX4_ERROR;
208+
return required ? false : true;
86209
}
87210

88-
// mag: /world/$WORLD/model/$MODEL/link/base_link/sensor/magnetometer_sensor/magnetometer
211+
return true;
212+
}
213+
214+
bool GZBridge::subscribeMag(bool required)
215+
{
89216
std::string mag_topic = "/world/" + _world_name + "/model/" + _model_name +
90217
"/link/base_link/sensor/magnetometer_sensor/magnetometer";
91218

92219
if (!_node.Subscribe(mag_topic, &GZBridge::magnetometerCallback, this)) {
93220
PX4_ERR("failed to subscribe to %s", mag_topic.c_str());
94-
return PX4_ERROR;
221+
return required ? false : true;
95222
}
96223

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

100232
if (!_node.Subscribe(odometry_topic, &GZBridge::odometryCallback, this)) {
101233
PX4_ERR("failed to subscribe to %s", odometry_topic.c_str());
102-
return PX4_ERROR;
234+
return required ? false : true;
103235
}
104236

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

108244
if (!_node.Subscribe(laser_scan_topic, &GZBridge::laserScanCallback, this)) {
109245
PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str());
246+
return required ? false : true;
110247
}
111248

112-
// Distance Sensor(AFBRS50): optional
249+
return true;
250+
}
251+
252+
bool GZBridge::subscribeDistanceSensor(bool required)
253+
{
113254
std::string lidar_sensor = "/world/" + _world_name + "/model/" + _model_name +
114255
"/link/lidar_sensor_link/sensor/lidar/scan";
115256

116257
if (!_node.Subscribe(lidar_sensor, &GZBridge::laserScantoLidarSensorCallback, this)) {
117258
PX4_WARN("failed to subscribe to %s", lidar_sensor.c_str());
259+
return required ? false : true;
118260
}
119261

120-
// Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed
262+
return true;
263+
}
264+
265+
bool GZBridge::subscribeAirspeed(bool required)
266+
{
121267
std::string airspeed_topic = "/world/" + _world_name + "/model/" + _model_name +
122268
"/link/airspeed_link/sensor/air_speed/air_speed";
123269

124270
if (!_node.Subscribe(airspeed_topic, &GZBridge::airspeedCallback, this)) {
125271
PX4_ERR("failed to subscribe to %s", airspeed_topic.c_str());
126-
return PX4_ERROR;
272+
return required ? false : true;
127273
}
128274

129-
// Air pressure: /world/$WORLD/model/$MODEL/link/base_link/sensor/air_pressure_sensor/air_pressure
275+
return true;
276+
}
277+
278+
bool GZBridge::subscribeAirPressure(bool required)
279+
{
130280
std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name +
131281
"/link/base_link/sensor/air_pressure_sensor/air_pressure";
132282

133-
if (!_node.Subscribe(air_pressure_topic, &GZBridge::barometerCallback, this)) {
283+
if (!_node.Subscribe(air_pressure_topic, &GZBridge::airPressureCallback, this)) {
134284
PX4_ERR("failed to subscribe to %s", air_pressure_topic.c_str());
135-
return PX4_ERROR;
285+
return required ? false : true;
136286
}
137287

138-
// GPS: /world/$WORLD/model/$MODEL/link/base_link/sensor/navsat_sensor/navsat
288+
return true;
289+
}
290+
291+
bool GZBridge::subscribeNavsat(bool required)
292+
{
139293
std::string nav_sat_topic = "/world/" + _world_name + "/model/" + _model_name +
140294
"/link/base_link/sensor/navsat_sensor/navsat";
141295

142296
if (!_node.Subscribe(nav_sat_topic, &GZBridge::navSatCallback, this)) {
143297
PX4_ERR("failed to subscribe to %s", nav_sat_topic.c_str());
144-
return PX4_ERROR;
298+
return required ? false : true;
145299
}
146300

301+
return true;
302+
}
303+
304+
bool GZBridge::subscribeOpticalFlow(bool required)
305+
{
147306
std::string flow_topic = "/world/" + _world_name + "/model/" + _model_name +
148307
"/link/flow_link/sensor/optical_flow/optical_flow";
149308

150309
if (!_node.Subscribe(flow_topic, &GZBridge::opticalFlowCallback, this)) {
151310
PX4_ERR("failed to subscribe to %s", flow_topic.c_str());
152-
return PX4_ERROR;
311+
return required ? false : true;
153312
}
154313

155-
if (!_mixing_interface_esc.init(_model_name)) {
156-
PX4_ERR("failed to init ESC output");
157-
return PX4_ERROR;
158-
}
159-
160-
if (!_mixing_interface_servo.init(_model_name)) {
161-
PX4_ERR("failed to init servo output");
162-
return PX4_ERROR;
163-
}
164-
165-
if (!_mixing_interface_wheel.init(_model_name)) {
166-
PX4_ERR("failed to init motor output");
167-
return PX4_ERROR;
168-
}
169-
170-
if (!_gimbal.init(_world_name, _model_name)) {
171-
PX4_ERR("failed to init gimbal");
172-
return PX4_ERROR;
173-
}
174-
175-
ScheduleNow();
176-
return OK;
314+
return true;
177315
}
178316

179317
void GZBridge::clockCallback(const gz::msgs::Clock &msg)
@@ -254,7 +392,7 @@ void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &msg)
254392
_sensor_mag_pub.publish(report);
255393
}
256394

257-
void GZBridge::barometerCallback(const gz::msgs::FluidPressure &msg)
395+
void GZBridge::airPressureCallback(const gz::msgs::FluidPressure &msg)
258396
{
259397
const uint64_t timestamp = hrt_absolute_time();
260398

@@ -273,7 +411,6 @@ void GZBridge::barometerCallback(const gz::msgs::FluidPressure &msg)
273411
_sensor_baro_pub.publish(report);
274412
}
275413

276-
277414
void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &msg)
278415
{
279416
const uint64_t timestamp = hrt_absolute_time();
@@ -495,7 +632,7 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &msg)
495632
_visual_odometry_pub.publish(report);
496633
}
497634

498-
static float generate_wgn()
635+
float GZBridge::generate_wgn()
499636
{
500637
// generate white Gaussian noise sample with std=1
501638

@@ -798,35 +935,6 @@ void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::m
798935
q_FRD_to_NED = q_ENU_to_NED * q_FLU_to_ENU * q_FLU_to_FRD.Inverse();
799936
}
800937

801-
void GZBridge::Run()
802-
{
803-
if (should_exit()) {
804-
ScheduleClear();
805-
806-
_mixing_interface_esc.stop();
807-
_mixing_interface_servo.stop();
808-
_mixing_interface_wheel.stop();
809-
_gimbal.stop();
810-
811-
exit_and_cleanup();
812-
return;
813-
}
814-
815-
if (_parameter_update_sub.updated()) {
816-
parameter_update_s pupdate;
817-
_parameter_update_sub.copy(&pupdate);
818-
819-
updateParams();
820-
821-
_mixing_interface_esc.updateParams();
822-
_mixing_interface_servo.updateParams();
823-
_mixing_interface_wheel.updateParams();
824-
_gimbal.updateParams();
825-
}
826-
827-
ScheduleDelayed(10_ms);
828-
}
829-
830938
int GZBridge::task_spawn(int argc, char *argv[])
831939
{
832940
std::string world_name;

0 commit comments

Comments
 (0)