[ROS2][BUG] Cannot obtain some params from PX4 in MAVROS2

Issue details

In some cases MAVROS2 cannot change some parameters. I'm using the service /mavros/param/set_parameters which returns a failure with reason Undeclared parameter. I know the parameter in question exists in the connected SITL autopilot, and if I restart the system sometimes it's other parameters that fails, sometimes not any.

I think the issue is linked to this warning I have in my logs.

[1652263157.093709350] [WARN] PR: request param #163 timeout, retries left 2, and 254 params still missing

In addition if I request the param list with one of the two lines below, I get 580 params in the response (including _HASH_CHECK).

$ ros2 service call /mavros/param/list_parameters rcl_interfaces/srv/ListParameters
$ ros2 param list

I know that my autopilot has 831 params (including _HASH_CHECK). Which approximately matches the 254 missing. So I'm wondering where those 254 parameters went... Subscribing to the /mavros/param/param_value topic, I see all of them published when MAVROS pulls them from autopilot 10s after boot (I created a small Python script to receive them with mavutil).

I tried to pull params with MAVROS service. If I don't force I get the 580 I had previously (as expected), and if I force pull I get usually more

root@3e646d6aeccf:/home/workspace# ros2 service call /mavros/param/pull mavros_msgs/srv/ParamPull force_pull:\ false\ 
waiting for service to become available...
requester: making request: mavros_msgs.srv.ParamPull_Request(force_pull=False)

response:
mavros_msgs.srv.ParamPull_Response(success=True, param_received=580)

root@3e646d6aeccf:/home/workspace# ros2 service call /mavros/param/pull mavros_msgs/srv/ParamPull force_pull:\ true\ 
waiting for service to become available...
requester: making request: mavros_msgs.srv.ParamPull_Request(force_pull=True)

response:
mavros_msgs.srv.ParamPull_Response(success=False, param_received=768)

root@3e646d6aeccf:/home/workspace# ros2 service call /mavros/param/pull mavros_msgs/srv/ParamPull force_pull:\ true\ 
waiting for service to become available...
requester: making request: mavros_msgs.srv.ParamPull_Request(force_pull=True)

response:
mavros_msgs.srv.ParamPull_Response(success=False, param_received=745)

MAVROS version and platform

Mavros: 2.1.1 ROS: ROS2 Galactic Ubuntu: 20.04

Autopilot type and version

[ ] ArduPilot [x] PX4

Node logs

[1652263106.522306206] [INFO] Starting mavros_node container
[1652263106.522559938] [INFO] FCU URL: udp://:14540@localhost:14557
[1652263106.522599119] [INFO] GCS URL: 
[1652263106.522620909] [INFO] UAS Prefix: /uas1
[1652263106.522639976] [INFO] Starting mavros router node
[1652263106.655211967] [INFO] Built-in SIMD instructions: SSE, SSE2
[1652263106.655337332] [INFO] Built-in MAVLink package version: 2022.3.3
[1652263106.655369319] [INFO] Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[1652263106.655396557] [INFO] MAVROS Router started
[1652263106.658817858] [INFO] Requested to add endpoint: type: 0, url: udp://:14540@localhost:14557
[1652263106.663006922] [INFO] Endpoint link[1000] created
[1652263106.668933630] [INFO] link[1000] opened successfully
[1652263106.669678275] [INFO] Requested to add endpoint: type: 2, url: /uas1
[1652263106.670194609] [INFO] Endpoint link[1001] created
[1652263106.676913384] [INFO] link[1001] opened successfully
[1652263106.678122471] [INFO] Starting mavros uas node
[1652263106.914009986] [INFO] UAS Executor started, threads: 8
[1652263107.202598547] [INFO] Plugin actuator_control created
[1652263107.203852542] [INFO] Plugin actuator_control initialized
[1652263107.508412484] [INFO] Plugin adsb created
[1652263107.509663895] [INFO] Plugin adsb initialized
[1652263107.636018640] [INFO] Plugin altitude created
[1652263107.636529876] [INFO] Plugin altitude initialized
[1652263107.636608657] [INFO] Plugin cam_imu_sync ignored
[1652263107.636716282] [INFO] Plugin camera ignored
[1652263107.758627323] [INFO] Plugin command created
[1652263107.759194222] [INFO] Plugin command initialized
[1652263107.759293536] [INFO] Plugin companion_process_status ignored
[1652263107.759328387] [INFO] Plugin debug_value ignored
[1652263107.832292754] [INFO] DS: Plugin not configured!
[1652263107.833823739] [INFO] Plugin distance_sensor created
[1652263107.834281686] [INFO] Plugin distance_sensor initialized
[1652263107.834376530] [INFO] Plugin esc_status ignored
[1652263107.834440505] [INFO] Plugin esc_telemetry ignored
[1652263107.834498333] [INFO] Plugin fake_gps ignored
[1652263107.834547361] [INFO] Plugin ftp ignored
[1652263107.908758251] [INFO] Plugin geofence created
[1652263107.909546477] [INFO] Plugin geofence initialized
[1652263108.008807241] [INFO] Plugin global_position created
[1652263108.009349905] [INFO] Plugin global_position initialized
[1652263108.009444400] [INFO] Plugin gps_input ignored
[1652263108.009480997] [INFO] Plugin gps_rtk ignored
[1652263108.009512705] [INFO] Plugin gps_status ignored
[1652263108.009537498] [INFO] Plugin hil ignored
[1652263108.150709711] [INFO] Plugin home_position created
[1652263108.151176319] [INFO] Plugin home_position initialized
[1652263108.243474214] [INFO] Plugin imu created
[1652263108.244245608] [INFO] Plugin imu initialized
[1652263108.244333468] [INFO] Plugin landing_target ignored
[1652263108.337057951] [INFO] Plugin local_position created
[1652263108.337758038] [INFO] Plugin local_position initialized
[1652263108.337839472] [INFO] Plugin log_transfer ignored
[1652263108.337873764] [INFO] Plugin mag_calibration_status ignored
[1652263108.337916158] [INFO] Plugin manual_control ignored
[1652263108.337958411] [INFO] Plugin mocap_pose_estimate ignored
[1652263108.338000316] [INFO] Plugin mount_control ignored
[1652263108.338041313] [INFO] Plugin nav_controller_output ignored
[1652263108.338085243] [INFO] Plugin obstacle_distance ignored
[1652263108.338122817] [INFO] Plugin odometry ignored
[1652263108.338159973] [INFO] Plugin onboard_computer_status ignored
[1652263108.427258590] [INFO] Plugin param created
[1652263108.428041438] [INFO] Plugin param initialized
[1652263108.428128321] [INFO] Plugin play_tune ignored
[1652263108.428164987] [INFO] Plugin px ignored
[1652263108.502994668] [INFO] Plugin rallypoint created
[1652263108.503490330] [INFO] Plugin rallypoint initialized
[1652263108.503615764] [INFO] Plugin rangefinder ignored
[1652263108.600343674] [INFO] Plugin rc_io created
[1652263108.600889482] [INFO] Plugin rc_io initialized
[1652263108.600974757] [INFO] Plugin setpoint_accel ignored
[1652263108.601021132] [INFO] Plugin setpoint_attitude ignored
[1652263108.601070090] [INFO] Plugin setpoint_position ignored
[1652263108.694105227] [INFO] Plugin setpoint_raw created
[1652263108.694552489] [INFO] Plugin setpoint_raw initialized
[1652263108.694622749] [INFO] Plugin setpoint_trajectory ignored
[1652263108.770861193] [INFO] Plugin setpoint_velocity created
[1652263108.770988722] [INFO] Plugin setpoint_velocity initialized
[1652263108.886858307] [INFO] Plugin sys_status created
[1652263108.888102384] [INFO] Plugin sys_status initialized
[1652263108.949307621] [INFO] TM: Timesync mode: MAVLINK
[1652263108.968250179] [INFO] Plugin sys_time created
[1652263108.968664196] [INFO] Plugin sys_time initialized
[1652263109.038864885] [INFO] Plugin tdr_radio created
[1652263109.039345112] [INFO] Plugin tdr_radio initialized
[1652263109.117176629] [INFO] Plugin terrain created
[1652263109.117531073] [INFO] Plugin terrain initialized
[1652263109.117624520] [INFO] Plugin trajectory ignored
[1652263109.117679834] [INFO] Plugin tunnel ignored
[1652263109.204845043] [INFO] Plugin vfr_hud created
[1652263109.205197740] [INFO] Plugin vfr_hud initialized
[1652263109.205278477] [INFO] Plugin vibration ignored
[1652263109.205337143] [INFO] Plugin vision_pose ignored
[1652263109.205438063] [INFO] Plugin vision_speed ignored
[1652263109.316270924] [INFO] Plugin waypoint created
[1652263109.317027441] [INFO] Plugin waypoint initialized
[1652263109.317170057] [INFO] Plugin wheel_odomotry ignored
[1652263109.395959094] [INFO] Plugin wind_estimation created
[1652263109.396422699] [INFO] Plugin wind_estimation initialized
[1652263109.403117588] [INFO] Built-in SIMD instructions: SSE, SSE2
[1652263109.403175486] [INFO] Built-in MAVLink package version: 2022.3.3
[1652263109.403207474] [INFO] Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[1652263109.403225143] [INFO] MAVROS UAS via /uas1 started. MY ID 1.191, TARGET ID 1.1
[1652263109.452896487] [INFO] link[1001] detected remote address 1.191
[1652263110.893824172] [WARN] VER: broadcast request timeout, retries left 4
[1652263111.905868147] [WARN] VER: broadcast request timeout, retries left 3
[1652263112.903577675] [WARN] VER: unicast request timeout, retries left 2
[1652263113.898203932] [WARN] VER: unicast request timeout, retries left 1
[1652263114.895724821] [WARN] VER: unicast request timeout, retries left 0
[1652263115.897140953] [WARN] VER: your FCU don't support AUTOPILOT_VERSION, switched to default capabilities
[1652263120.689163768] [INFO] link[1000] detected remote address 1.1
[1652263120.691441423] [INFO] IMU: High resolution IMU detected!
[1652263121.321721742] [INFO] CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[1652263121.322230464] [INFO] WP: detected enable_partial_push: 0
[1652263121.328671830] [INFO] IMU: High resolution IMU detected!
[1652263122.342777100] [INFO] GF: Using MISSION_ITEM_INT
[1652263122.342882630] [INFO] RP: Using MISSION_ITEM_INT
[1652263122.342927537] [INFO] WP: Using MISSION_ITEM_INT
[1652263122.342979499] [INFO] VER: 1.1: Capabilities         0x000000000000e4ef
[1652263122.343034743] [INFO] VER: 1.1: Flight software:     010c0100 (b4aab2ae30010c04)
[1652263122.343068546] [INFO] VER: 1.1: Middleware software: 010c0100 (b4aab2ae30000000)
[1652263122.343098578] [INFO] VER: 1.1: OS software:         050d00ff (76bb42f3ebd90210)
[1652263122.343134267] [INFO] VER: 1.1: Board hardware:      00000001
[1652263122.343166743] [INFO] VER: 1.1: VID/PID:             0000:0000
[1652263122.343193073] [INFO] VER: 1.1: UID:                 4954414c44494e4f
[1652263122.354647348] [WARN] CMD: Unexpected command 520, result 0
[1652263122.977372128] [INFO] IMU: Attitude quaternion IMU detected!
[1652263127.090397678] [INFO] PR: start force pull
[1652263129.746007747] [WARN] TM: RTT too high for timesync: 1388.37 ms.
[1652263130.102822318] [WARN] TM: RTT too high for timesync: 650.21 ms.
[1652263136.323789991] [INFO] WP: mission received
[1652263141.334339666] [INFO] RP: mission received
[1652263146.323492797] [INFO] GF: mission received
[1652263157.093709350] [WARN] PR: request param #163 timeout, retries left 2, and 254 params still missing
[1652263351.656437654] [ERROR] PR: Unknown parameter to set: NAV_DLL_ACT

If i remember correctly some APs can skip sending some params for disabled blocks. Unsure if that is the case.

Normally after PARAM_REQUEST_LIST AP should send all existing PARAM_VALUE's. But sometimes it possible to miss some, so plugin uses first PV count to estimate all indexes, then run re-request loop of absent PVs by index.

Could you please enable debug logging for the param plugin logger and then force pull? Need to check param_index vs param_count.

While trying to debug I had things working more or less nicely except for the force param pull, not sure how related it is. In any case the first issue did not arrive in this case, will continue debugging to reproduce.

I could not find how to switch only the param plugin to debug, so I switched all of MAVROS and grep-ed the PR lines only. The output is in my gist here.

We can see that at connection of PX4 (ts=1652337616) I get a few params already. Then 10s after connection (as defined by BOOTUP_TIME) I get the full 831 params (ts=1652337626->1652337631).

At this moment triggering a non-forced pull gave me the full param list:

root@victor-ThinkPad-E595:/home/workspace# ros2 service call /mavros/param/pull mavros_msgs/srv/ParamPull 
requester: making request: mavros_msgs.srv.ParamPull_Request(force_pull=False)
response:
mavros_msgs.srv.ParamPull_Response(success=True, param_received=832)

However, after this if I forced pull (at ts=1652337667) I received only 485 params at first (up to ts=1652337669), then the service call timed out and returned me this:

root@victor-ThinkPad-E595:/home/workspace# ros2 service call /mavros/param/pull mavros_msgs/srv/ParamPull force_pull:\ true\ 
waiting for service to become available...
requester: making request: mavros_msgs.srv.ParamPull_Request(force_pull=True)
response:
mavros_msgs.srv.ParamPull_Response(success=False, param_received=485)

And after 30s (LIST_TIMEOUT) the param plugin got the rest (ts=1652337697).

I found my issue!

Basically I had another ROS2 node that was sending some param pull requests with force=true and I think it messed up the params sent in the param topic.

I'll mark as complete

@Vicidel, I am trying to set parameters, but the response I get from the service is always success=False. Can you share with me the command you are using in the terminal (or the snippet of code) were you are able to successfully set a PX4 param?

I've tried using set and set_params. Neither work (although my get commands do work). I am using PX4 Gazebo with a px4_sitl_rtps vehicle. I am using ROS2 foxy.

Examples below:

# Set
ros2 service call /mavros/param/set mavros_msgs/srv/ParamSetV2 "{'force_set': True, 'param_id': 'BAT_CRIT_THR', 'value': {'double_value': 0.90}}"
requester: making request: mavros_msgs.srv.ParamSetV2_Request(force_set=True, param_id='BAT_CRIT_THR', value=rcl_interfaces.msg.ParameterValue(type=0, bool_value=False, integer_value=0, double_value=0.9, string_value='', byte_array_value=[], bool_array_value=[], integer_array_value=[], double_array_value=[], string_array_value=[]))

response:
mavros_msgs.srv.ParamSetV2_Response(success=False, value=rcl_interfaces.msg.ParameterValue(type=0, bool_value=False, integer_value=0, double_value=0.9, string_value='', byte_array_value=[], bool_array_value=[], integer_array_value=[], double_array_value=[], string_array_value=[]))

# SetParameters
ros2 service call /mavros/param/set_parameters rcl_interfaces/srv/SetParameters "{parameters: [{name: BAT_CRIT_THR, value: {double_value: 0.09}}]}"
requester: making request: rcl_interfaces.srv.SetParameters_Request(parameters=[rcl_interfaces.msg.Parameter(name='BAT_CRIT_THR', value=rcl_interfaces.msg.ParameterValue(type=0, bool_value=False, integer_value=0, double_value=0.09, string_value='', byte_array_value=[], bool_array_value=[], integer_array_value=[], double_array_value=[], string_array_value=[]))])

response:
rcl_interfaces.srv.SetParameters_Response(results=[rcl_interfaces.msg.SetParametersResult(successful=False, reason='')])

Hey @jaredsjohansen, indeed when I run your commands they both fails too. I have a print in MAVROS [WARN] PR: Unsupported ParameterValue type: not set

Trying with standard ROS2 CLI works:

root@d9d0b0d6bd2b:/home/workspace# ros2 param get /mavros/param BAT_CRIT_THR
Double value is: 0.05000000074505806
root@d9d0b0d6bd2b:/home/workspace# ros2 param set /mavros/param BAT_CRIT_THR 0.06
Set parameter successful
root@d9d0b0d6bd2b:/home/workspace# ros2 param get /mavros/param BAT_CRIT_THR
Double value is: 0.05999999865889549

If I change it from my code it also works, but I changed my method to set them to a third method as the two other mentioned above: AsyncParametersClient. You have some example on how to use them in demo nodes, but here is my code. It might be overcomplicated in this form because I'm converting it from my class structure to simplify things

// define client
rclcpp::AsyncParametersClient::SharedPtr paramClientMavros = std::make_shared<rclcpp::AsyncParametersClient>(this, "/mavros/param");

// wait for param client
if (!paramClientMavros->wait_for_service(2s))
{
    std::cout << "Param server not available" << std::endl;
    return false;
}

// create request
std::vector<rclcpp::Parameter> parameters_to_change;
parameters_to_change.emplace_back("BAT_CRIT_THR", rclcpp::ParameterValue(0.06));

// set the params (answer type is https://github.com/ros2/rcl_interfaces/blob/foxy/rcl_interfaces/msg/SetParametersResult.msg)
auto set_param_result = paramClientMavros->set_parameters(parameters_to_change);
if (set_param_result.wait_for(2s) == std::future_status::ready)
{
    // failed
    auto future_response = set_param_result.get();
    if (!future_response.front().successful)
    {[
        std::cout << "Param set failed" << std::endl;
        return false;
    }
}
// timeout
else
{
    std::cout << "Param set timed out" << std::endl;
    return false;
}

// all good
return true;

Hope it helps, but no clue why the CLI service call fails...

Thanks, @Vicidel, for the guidance!

I've spent some time investigating everything you've shared. I'll share some of my takeaways (about mavros in general; and this problem in specific) for anyone who comes this way:

I never figured out why the CLI service fails.
You cannot have namespace=namespace, as one of the Node parameters (in a python launch file).
- If you do, the mavros topics/services do not show up. This means that mavros topics/services are always at /mavros/.
- One effect of this is that you can only use mavros with a single drone. This limitation works fine for single-drone cases, as well as for swarms in the real-world (where each vehicle has its own instance of mavros running), but does not work for swarm development/testing in simulation -- which is what I am doing. (I wanted a separate instance for each simulated vehicle, but could not instantiate a separate instance of mavros for each vehicle in its own namespace.)
There seems to be some limitation in having both the (recommended) micrortps_agent running alongside mavros2.
- If I start the micrortps_agent before the mavros rosnode, then mavros topics/services don't always show up (or a limited set of them do). I'm not sure why this is. I haven't investigated this much.
- (Note: The reason why I want to use both is the following: I have been using the micrortps_bridge for rapid communication between the FC and my SW. I tried to use this same mechanism to get/set PX4 params, but the micrortps_bridge doesn't support this capability. As a result, I turned to mavros2 to fill that gap. The purpose of getting/setting PX4 params was to perform some basic preflight checks prior to takeoff to ensure the vehicle has the PX4 settings you are expecting. That way the same piece of code can be run on different pieces of HW with the same expected performance.)
In ros2-foxy, ros2-galatic, and ros2-humble, AsyncParametersClient only has a cpp implementation; the python version doesn't exist until ros2-rolling. Based on @Vicidel shared, it is possible to use this mechanism to set parameters. I haven't developed a cpp ros2node to use this mechanism yet, but I appreciate @Vicidel's support in sharing what he's done to get it to work!

For my preflight check, I ended up using the limited capabilities of mavros to get certain PX4 parameters. I compare the results against expected values and inform the operator of the results. Most relevant code is below:

from rcl_interfaces.srv import GetParameters

    def get_px4_params(self, px4_params_to_get): # px4_params_to_get is a list of strings
        # Get PX4 params
        self.get_logger().info('Getting PX4 params')
        self.px4_client = self.create_client(GetParameters, '/mavros/param/get_parameters', callback_group=self.client_callback_group)
        while not self.px4_client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('/mavros/param/get_parameters service not available, waiting again...')
        request = GetParameters.Request()
        request.names = px4_params_to_get
        result = self.perform_service(self.px4_client, request)
        result_dict = {}
        for i in range(len(result.values)):
            param = px4_params_to_get[i]
            v = result.values[i]
            value = None
            if   v.type == 1: value = v.bool_value
            elif v.type == 2: value = v.integer_value
            elif v.type == 3: value = v.double_value
            elif v.type == 4: value = v.string_value
            elif v.type == 5: value = v.byte_array_value
            elif v.type == 6: value = v.bool_array_value
            elif v.type == 7: value = v.integer_array_value
            elif v.type == 8: value = v.double_array_value
            elif v.type == 9: value = v.string_array_value
            result_dict[param] = value
        return result_dict

    def perform_service(self, client, request):
        self.service_done_event.clear()
        event=Event()
        def done_callback(future):
            nonlocal event
            event.set()
        future = client.call_async(request)
        future.add_done_callback(done_callback)
        event.wait()
        return future.result()

If anyone finds a nice solution to this problem in the future, please share! I'd be interested in it!

mavlink / mavros