Hello, i am new to gazebo. My idea is to use it to experiment with drone autonomous flight and path finding. I am on Ubuntu 24.04 and using Gazebo 8.7.0 Harmonic. At this point i have created my world with quadcopter model which i am trying to control. Firstly, i wanted to make it hover at exact altitude. I thought it would be great first step to find out how to control it using cpp scripts and then create functions for particular movement (up, down, hover, forward etc…). But i am really struggling with it. Idea was to make him hover at 3 meters, but now just oscillates between ground and 12 meters. I tried many ways to change the parameters but it differs just a bit and still oscillates a lot.
Is my approach wrong? Or is it impossible to do so with pure gazebo without ros or some similar extension/library? I would be grateful for any advice.
- code of lidar_control.cpp:
#include <gz/msgs/actuators.pb.h>
#include <gz/msgs/altimeter.pb.h>
#include <gz/msgs/imu.pb.h>
#include <gz/transport/Node.hh>
#include
#include
#include
#include
#include
#include
#include// Define actuator and transport
gz::msgs::Actuators actuatorMsg;
gz::transport::Node node;// Target and control variables
double targetAltitude = 3.0; // Target altitude in meters
std::atomic currentAltitude(0.0); // Current altitude (updated via callback)
std::atomic currentVelocity(0.0); // Current velocity estimate
std::atomic currentMotorSpeed(700.0); // Current motor speed estimate
std::atomic stopHovering(false); // Stop hovering flag// PID Controller parameters
const double Kp = 5.0; // Proportional gain
const double Ki = 0.5; // Integral gain
const double Kd = 1.0; // Derivative gain
double integralError = 0.0; // Accumulated integral error// Speed bounds for adaptive adjustment
double maxSpeed = 750.0; // Maximum motor speed
double minSpeed = 500.0; // Minimum motor speed// Publish motor speeds
void publishMotorSpeed(const std::vector &speeds) {
actuatorMsg.mutable_velocity()->Clear();
for (const auto &speed : speeds) {
actuatorMsg.add_velocity(speed);
}
static auto publisher = node.Advertisegz::msgs::Actuators(“/X3/gazebo/command/motor_speed”);
publisher.Publish(actuatorMsg);
}// Altimeter callback
void altimeterCallback(const gz::msgs::Altimeter &msg) {
currentAltitude.store(msg.vertical_position());
}// IMU callback (for velocity estimation)
void imuCallback(const gz::msgs::IMU &msg) {
currentVelocity.store(msg.linear_acceleration().z()); // Assuming Z is upward
}// PID control loop for altitude
void controlAltitude() {
double previousError = 0.0; // Error in the previous loop
double previousAltitude = currentAltitude.load(); // Store for derivative calculationwhile (!stopHovering.load()) {
double currentAlt = currentAltitude.load();
double altitudeError = targetAltitude - currentAlt;
// Calculate derivative (rate of change of altitude)
double altitudeDerivative = (currentAlt - previousAltitude) / 0.1; // Derivative over 100ms
previousAltitude = currentAlt;
// Update integral error
integralError += altitudeError * 0.1; // Integrate over 100ms
// Compute PID output
double speedAdjustment = (Kp * altitudeError) +
(Ki * integralError) -
(Kd * altitudeDerivative);
// Adjust motor speed
double newSpeed = currentMotorSpeed.load() + speedAdjustment;
newSpeed = std::clamp(newSpeed, minSpeed, maxSpeed); // Clamp to allowable range
currentMotorSpeed.store(newSpeed);
// Adaptive adjustment of speed bounds
if (altitudeError > 0) {
minSpeed = std::max(minSpeed - 1.0, 500.0); // Decrease min speed
maxSpeed = std::min(maxSpeed + 1.0, 750.0); // Increase max speed
} else {
minSpeed = std::min(minSpeed + 1.0, 750.0); // Increase min speed
maxSpeed = std::max(maxSpeed - 1.0, 500.0); // Decrease max speed
}
// Set motor speeds
std::vector<double> motorSpeeds(4, currentMotorSpeed.load());
publishMotorSpeed(motorSpeeds);
// Print telemetry
std::cout << "Altitude: " << currentAlt << " m, "
<< "Motor Speed: " << currentMotorSpeed.load() << " RPM, "
<< "Error: " << altitudeError << " m\n";
// Sleep for loop interval
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
} // Telemetry printer thread void telemetryPrinter() { while (!stopHovering.load()) { std::cout << “Current Altitude: " << currentAltitude.load() << " m | Vertical Velocity: " << currentVelocity.load() << " m/s\n”; std::this_thread::sleep_for(std::chrono::seconds(1)); } } // Main function int main(int argc, char **argv) { std::cout << “Starting drone control program…\n”; std::cout << “Hovering target altitude: " << targetAltitude << " meters.\n”;// Subscribe to altimeter and IMU topics
if (!node.Subscribe(“/quadcopter/altimeter”, altimeterCallback)) {
std::cerr << “Error subscribing to /quadcopter/altimeter\n”;
return -1;
}
if (!node.Subscribe(“/imu”, imuCallback)) {
std::cerr << “Error subscribing to /imu\n”;
return -1;
}
// Start telemetry and altitude control threads
std::thread telemetryThread(telemetryPrinter);
std::thread altitudeControlThread(controlAltitude);
// Main thread just waits for exit signal
std::cout << “Hovering indefinitely at " << targetAltitude << " meters.\n”;
std::cout << “Press Ctrl+C to stop the program.\n”;
// Join threads (this blocks indefinitely unless stopHovering is set to true)
if (telemetryThread.joinable()) {
telemetryThread.join();
}
if (altitudeControlThread.joinable()) {
altitudeControlThread.join();
}
return 0;
}
- Here is piece of output in terminal:
Starting drone control program…
Hovering target altitude: 3 meters.
Current Altitude: 0 m | Vertical Velocity: 0 m/s
Hovering indefinitely at 3 meters.
Press Ctrl+C to stop the program.
Altitude: 0 m, Motor Speed: 715.15 RPM, Error: 3 m
Altitude: 0.00383934 m, Motor Speed: 730.392 RPM, Error: 2.99616 m
Altitude: 0.00875659 m, Motor Speed: 745.749 RPM, Error: 2.99124 m
Altitude: 0.0306002 m, Motor Speed: 750 RPM, Error: 2.9694 m
Altitude: 0.0804911 m, Motor Speed: 750 RPM, Error: 2.91951 m
Altitude: 0.149307 m, Motor Speed: 750 RPM, Error: 2.85069 m
Altitude: 0.277491 m, Motor Speed: 750 RPM, Error: 2.72251 m
Altitude: 0.377884 m, Motor Speed: 750 RPM, Error: 2.62212 m
Altitude: 0.546984 m, Motor Speed: 750 RPM, Error: 2.45302 m
Altitude: 0.735265 m, Motor Speed: 750 RPM, Error: 2.26473 m
Current Altitude: 0.776293 m | Vertical Velocity: 12.6541 m/s
Altitude: 0.776293 m, Motor Speed: 750 RPM, Error: 2.22371 m
Altitude: 1.0078 m, Motor Speed: 750 RPM, Error: 1.9922 m
Altitude: 1.48009 m, Motor Speed: 750 RPM, Error: 1.51991 m
Altitude: 1.77614 m, Motor Speed: 750 RPM, Error: 1.22386 m
Altitude: 2.11499 m, Motor Speed: 750 RPM, Error: 0.885009 m
Altitude: 2.48226 m, Motor Speed: 750 RPM, Error: 0.517741 m
Altitude: 2.86556 m, Motor Speed: 748.654 RPM, Error: 0.13444 m
Altitude: 3.29317 m, Motor Speed: 744.711 RPM, Error: -0.293174 m
Altitude: 3.73413 m, Motor Speed: 738.394 RPM, Error: -0.734127 m
Altitude: 4.22683 m, Motor Speed: 729.034 RPM, Error: -1.22683 m
Current Altitude: 4.61416 m | Vertical Velocity: 12.6102 m/s
Altitude: 4.70245 m, Motor Speed: 717.382 RPM, Error: -1.70245 m
Altitude: 5.22517 m, Motor Speed: 702.535 RPM, Error: -2.22517 m
Altitude: 5.7622 m, Motor Speed: 684.72 RPM, Error: -2.7622 m
Altitude: 6.30252 m, Motor Speed: 664.007 RPM, Error: -3.30252 m
Altitude: 6.84843 m, Motor Speed: 640.315 RPM, Error: -3.84843 m
Altitude: 7.29151 m, Motor Speed: 615.222 RPM, Error: -4.29151 m
Altitude: 7.8373 m, Motor Speed: 586.13 RPM, Error: -4.8373 m
Altitude: 8.36692 m, Motor Speed: 554.284 RPM, Error: -5.36692 m
Altitude: 8.88994 m, Motor Speed: 519.595 RPM, Error: -5.88994 m
Altitude: 9.39949 m, Motor Speed: 512 RPM, Error: -6.39949 m
Current Altitude: 9.74566 m | Vertical Velocity: 7.61798 m/s
Altitude: 9.80343 m, Motor Speed: 513 RPM, Error: -6.80343 m
Altitude: 10.2414 m, Motor Speed: 514 RPM, Error: -7.24141 m
Altitude: 10.6031 m, Motor Speed: 515 RPM, Error: -7.60312 m
Altitude: 10.9169 m, Motor Speed: 516 RPM, Error: -7.91694 m
Altitude: 11.2206 m, Motor Speed: 517 RPM, Error: -8.22059 m
Altitude: 11.4665 m, Motor Speed: 518 RPM, Error: -8.46652 m
Altitude: 11.6701 m, Motor Speed: 519 RPM, Error: -8.67014 m
Altitude: 11.8719 m, Motor Speed: 520 RPM, Error: -8.87185 m
Altitude: 12.0187 m, Motor Speed: 521 RPM, Error: -9.01873 m
Altitude: 12.1063 m, Motor Speed: 522 RPM, Error: -9.10631 m
Current Altitude: 12.1225 m | Vertical Velocity: 6.07472 m/s
Altitude: 12.1619 m, Motor Speed: 523 RPM, Error: -9.16185 m
Altitude: 12.2037 m, Motor Speed: 524 RPM, Error: -9.20366 m
Altitude: 12.1958 m, Motor Speed: 525 RPM, Error: -9.1958 m
Altitude: 12.1618 m, Motor Speed: 526 RPM, Error: -9.16175 m
Altitude: 12.1051 m, Motor Speed: 527 RPM, Error: -9.1051 m
Altitude: 11.9963 m, Motor Speed: 528 RPM, Error: -8.99627 m
Altitude: 11.8468 m, Motor Speed: 529 RPM, Error: -8.84678 m
Altitude: 11.6538 m, Motor Speed: 530 RPM, Error: -8.65376 m
Altitude: 11.4551 m, Motor Speed: 531 RPM, Error: -8.45511 m
Altitude: 11.2243 m, Motor Speed: 532 RPM, Error: -8.22433 m
Current Altitude: 11.0358 m | Vertical Velocity: 6.31622 m/s
Altitude: 10.9415 m, Motor Speed: 533 RPM, Error: -7.94146 m
If you need more info about the projetc, feel free to ask.