I want to upload a polygon as a geofence to a Flightmission. The coordinates of the geofence are set in an UI of the project. But when it comes to uploading the geofence, it crashes. Every other step from setting a geofence works fine.
See the following picture for Application Output:
I also attach my Code for creating and uploading Geofence here:
// //Helperfunction
std::string DronesSingleton::geofenceResultToString(mavsdk::Geofence::Result result) {
switch (result) {
case mavsdk::Geofence::Result::Success:
return "Success";
case mavsdk::Geofence::Result::Error:
return "Error";
case mavsdk::Geofence::Result::TooManyGeofenceItems:
return "Too many geofence items";
case mavsdk::Geofence::Result::Timeout:
return "Timeout";
case mavsdk::Geofence::Result::InvalidArgument:
return "Invalid argument";
case mavsdk::Geofence::Result::NoSystem:
return "No system";
default:
return "Unknown";
}
}
void DronesSingleton::createAndUploadGeofence(std::shared_ptr<mavsdk::Geofence> geofence)
{
if (m_geofence_.isEmpty()) {
std::cerr << "Geofence coordinates are empty. Cannot create geofence." << std::endl;
return;
}
std::vector<mavsdk::Geofence::Point> points;
std::vector<mavsdk::Geofence::Polygon> polygons;
// Iterate over the geofence coordinates and add them to the points vector
for (const auto& coordinate : m_geofence_) {
mavsdk::Geofence::Point point;
point.latitude_deg = coordinate.latitude();
point.longitude_deg = coordinate.longitude();
points.push_back(point);
std::cout << "Added geofence point: (" << point.latitude_deg << ", " << point.longitude_deg << ")" << std::endl;
}
if (points.empty()) {
std::cerr << "No valid geofence points were created." << std::endl;
return;
}
// Create the polygon
std::cout << "Creating the polygon." << std::endl;
mavsdk::Geofence::Polygon new_polygon{};
new_polygon.fence_type = mavsdk::Geofence::FenceType::Inclusion;
new_polygon.points = points;
polygons.push_back(new_polygon);
if(new_polygon.points.empty()) {
std::cerr << "No points were added." << std::endl;
}
else{
std::cout << "Polygon is fine, continue with GeofenceData." << std::endl;
}
// Prepare the GeofenceData
mavsdk::Geofence::GeofenceData geofence_data{};
geofence_data.polygons = polygons;
if(geofence_data.polygons.empty()) {
std::cerr << "No data added." << std::endl;
}
else{
std::cout << "Data is fine, continue with upload." << std::endl;
}
// Upload the geofence
const mavsdk::Geofence::Result result = geofence->upload_geofence(geofence_data);
if (result != mavsdk::Geofence::Result::Success) {
std::cerr << "Failed to upload geofence: " << geofenceResultToString(result) << std::endl;
} else {
std::cout << "Geofence successfully uploaded!" << std::endl;
}
}
[...]
void Dronessingleton::start()
{
[...]
auto geofence = std::make_shared<mavsdk::Geofence>(system);
[...]
}
I use:
- px4_msgs v2.0.1
- PX4-Autopilot v1.14.0