diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index e8bccfa1..6fb9240a 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -24,9 +24,9 @@ public: HeartBeat() : Node("heartbeat") { // disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - vehicle_control_mode_service_ = this->create_service("drone/set_vehicle_control", std::bind()) - // create a publisher on the offboard control mode topic - offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + vehicle_control_mode_service_ = this->create_service("drone/set_vehicle_control", std::bind(&Heartbeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + // create a publisher on the offboard control mode topic + offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); // create timer to send heartbeat messages (offboard control) every 100ms timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this)); start_time = this->get_clock()->now().seconds(); @@ -66,7 +66,7 @@ private: } void handle_vehicle_control_change( - const std::shared_ptr request_header, + const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) {