Random values when sending mode switch request in SITL with ROS2

[jvzacchi]

Bug report

Issue details

’m currently in the process of setting up a ROS2 environment to communicate with Pixhawk 6x. For the moment, I’m using SITL to test things before going to HW. So far, the AP_DDS works apart from the ROS2 services.

Arm Motors works (arm throttle SUCCESS), but result is never sent back after the request.

Mode Switch has a very weird behavior. When sending an integer mode, let’s say 4 ROVER_MODE_GUIDED, the message will arrive , but a complete random number and it will fail because that (random) mode does not exist.

I have tested this both with python and cpp ROS2 nodes. Bellow you can find my cpp node snipet for the request.

Version
Rover

Platform
[ x ] All
[ ] AntennaTracker
[ ] Copter
[ ] Plane
[ ] Rover
[ ] Submarine

Airframe type
Rover

Hardware type
SITL

Logs

void call_mode_switch() {
        if (!mode_switch_client_->wait_for_service(std::chrono::seconds(1))) {
            RCLCPP_INFO(this->get_logger(), "Service not available, waiting again...");
            return;
        }

        auto request = std::make_shared<ardupilot_msgs::srv::ModeSwitch::Request>();
        uint8_t mode = 4;
        request->mode = mode;

        auto result_future = mode_switch_client_->async_send_request(request);
    }

@Ryanf55

[Ryanf55]

Can you supply which version or commit hash you were running of ArduPilot to reproduce this? "Rover" is not a version.

[jvzacchi]

@Ryanf55 0736106

[Ryanf55]

@arshPratap Do you have a chance to look at this bug?

[jvzacchi]

Just an update. I moved to HW and the error is not present when using serial there.

[Ryanf55]

Running the agent

ros2 run micro_ros_agent micro_ros_agent udp4 -p 2019 -r dds_xrce_profile.xml

Plane

 ./Tools/autotest/sim_vehicle.py -v Plane --map --console --enable-dds

If you call arming right away, before the vehicle is booted, it properly reports a failure.

 ros2 service call /ap/arm_motors ardupilot_msgs/srv/ArmMotors "{arm: true}"

If you call it later once it's properly initialized, it reports a success.

For plane, you can set the mode to 4, and it switches mode correctly.

ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 4}"

I tested modes 1-9. All work. 9 gets rejected, with the current mode reported as 8, and the logs show "AP: No such mode 9"

Copter

 ./Tools/autotest/sim_vehicle.py -v Plane --map --console --enable-dds

Requesting all the mode changes works as expected.

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 0}"
waiting for service to become available...
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=0)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=0)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 1}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=1)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=1)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 2}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=2)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=2)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 3}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=3)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=3)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 4}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=4)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 5}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=5)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=5)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 6}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=6)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=6)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 7}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=7)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=7)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 8}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=8)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=False, curr_mode=7)

Rover logs

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/arm_motors ardupilot_msgs/srv/ArmMotors "{arm: true}"
requester: making request: ardupilot_msgs.srv.ArmMotors_Request(arm=True)

response:
ardupilot_msgs.srv.ArmMotors_Response(result=True)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 1}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=1)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=1)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 2}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=2)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=False, curr_mode=1)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 3}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=3)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=3)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 4}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=4)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 5}"
waiting for service to become available...
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=5)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=5)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 6}"
waiting for service to become available...
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=6)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=False, curr_mode=5)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 7}"
waiting for service to become available...
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=7)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=7)

ryan@B650-970:~/Dev/ardu_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 8}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=8)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=False, curr_mode=7)

Conclusion

I can't reproduce on SITL. Please re-open if you can provide some steps like this to reproduce, otherwise I suspect it's an issue in your code.