diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 716e36cd..d989b02c 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -24,7 +24,8 @@ 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(&HeartBeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + 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)); + RCLCPP_INFO(this->get_logger(), "initialized service /drone/set_vehicle_control"); // 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