diff --git a/clover/CMakeLists.txt b/clover/CMakeLists.txt index d779e5f7..baac2344 100644 --- a/clover/CMakeLists.txt +++ b/clover/CMakeLists.txt @@ -82,6 +82,7 @@ add_service_files( GetTelemetry.srv Navigate.srv NavigateGlobal.srv + TakeOff.srv SetPosition.srv SetVelocity.srv SetAttitude.srv diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp index bc66d511..0d457d88 100644 --- a/clover/src/simple_offboard.cpp +++ b/clover/src/simple_offboard.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -781,6 +782,10 @@ bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); } +bool takeOff(TakeOff::Request& req, TakeOff::Response& res) { + return serve(NAVIGATE, 0, 0, req.alt, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, 0, NAN, NAN, NAN, req.speed, body.child_frame_id, true, res.success, res.message); +} + bool setPosition(SetPosition::Request& req, SetPosition::Response& res) { return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); } @@ -905,6 +910,7 @@ int main(int argc, char **argv) auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry); auto na_serv = nh.advertiseService("navigate", &navigate); auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal); + auto to_serv = nh.advertiseService("take_off", &takeOff); auto sp_serv = nh.advertiseService("set_position", &setPosition); auto sv_serv = nh.advertiseService("set_velocity", &setVelocity); auto sa_serv = nh.advertiseService("set_attitude", &setAttitude); diff --git a/clover/srv/TakeOff.srv b/clover/srv/TakeOff.srv new file mode 100644 index 00000000..a46c3d80 --- /dev/null +++ b/clover/srv/TakeOff.srv @@ -0,0 +1,5 @@ +float32 alt +float32 speed +--- +bool success +string message