The drone doesn't response to /mavros/setpoint_position/local messages

PX4 Autopilot
1

Hello,

I have a workspace with “Firmware” & “PX4-SITL_gazebo-classic” packages. I have included another package to control the drone comes by launching roslaunch px4 mavros_posix_sitl.launch. So, I created a c++ node that publishes a new position to mavros/setpoin_position/local, and I use QGroundControl first to take off the drone. The drone does not response once I run the node; however, I find the message already published to the topic mavros/setpoint_position/local! Also, the echo of mavros/state topic shows the current mode is mode: "AUTO.LOITER.

Here is the code I am using:


int main(int argc, char** argv)
{
    // Initialize the node
    ros::init(argc, argv, "drone_forward");
    ros::NodeHandle nh;

    // Create the arming client
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");

    // Create the mode client
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");

    // Create the setpoint position publisher
    ros::Publisher setpoint_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_position/local", 10);

    // Wait for the services to become available
    arming_client.waitForExistence();
    set_mode_client.waitForExistence();

    
    // Arm the drone
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    if (!arming_client.call(arm_cmd)) {
        ROS_ERROR("Failed to arm the drone");
        return 1;
    }

    // Switch to OFFBOARD mode
    mavros_msgs::SetMode offboard_mode_cmd;
    offboard_mode_cmd.request.custom_mode = "OFFBOARD";
    if (!set_mode_client.call(offboard_mode_cmd)) {
        ROS_ERROR("Failed to set the flight mode to OFFBOARD");
        return 1;
    }

    // Move the drone to a position 5 meters forward from the current position
    geometry_msgs::PoseStamped pose_cmd;
    pose_cmd.header.stamp = ros::Time::now();
    pose_cmd.header.frame_id = "map";
    pose_cmd.pose.position.x = 5.0;
    setpoint_pub.publish(pose_cmd);


    return 0;
}

In the code, I tried first to not change the mode and only publish the message, but the drone didn’t response. Hence, I switch the mode but I get this message on the terminal: CMD: Unexpected command 176, result 1.

I am new to the package, so excuse me if my explanation isn’t clear.