MAVROS2: /mavros/mission/push - Possible Bug: no response from Service

[Scoeerg]

Issue details

I would like to send Waypoints in MAVROS2, here's a minimal working example based on this GIT Mavros Issue:

# see https://github.com/mavlink/mavros/issues/1718
import rclpy
from rclpy.node import Node
from mavros_msgs.srv import WaypointPush
from mavros_msgs.msg import Waypoint
from mavros_msgs.msg import WaypointList

class WaypointPushNode(Node):
    def __init__(self):
        super().__init__('waypoint_push_node')
        waypoint_push_client = self.create_client(WaypointPush, '/mavros/mission/push')

        # See /mavros/mavros_msgs/msg/Waypoint.msg and /mavros/mavros_msgs/msg/WaypointList.msg
        waypoint_list = WaypointList()
        waypoint = Waypoint()
        # See https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_WAYPOINT
        waypoint.frame = 0 # frame 6 = Global relative altitude (to home position)
        waypoint.command = 16 # command 16 = MAV_CMD_NAV_WAYPOINT -> go to waypoint
        waypoint.is_current = False
        waypoint.autocontinue = True
        # Hold time (how long does the vehicle stay at the waypoint in seconds):
        waypoint.param1 = float(0)
        # Accept Radius - how close you need to be to mark waypoint as reached in meters:
        waypoint.param2 = float(0)
        # 0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for
        # counter-clockwise orbit. Allows trajectory control:
        waypoint.param3 = float(0)
        # Desired yaw angle at waypoint (rotary wing). NaN to use the current system yaw heading mode
        # (e.g. yaw towards next waypoint, yaw to home, etc.):
        waypoint.param4 = float("nan")
        # Latitude:
        waypoint.x_lat = float(50.00000)
        # Longitude:
        waypoint.y_long = float(7.00000)
        # Altitude in meters:
        waypoint.z_alt = float(0)
        # Append
        #waypoint_list.waypoints.append(waypoint)
        # Populate your request
        request = WaypointPush.Request()
        # See /mavros/mavros_msgs/srv/WaypointPush.srv
        request.start_index = 0
        request.waypoints.append(waypoint)
        self.get_logger().info('Waypoint sending: %s.' %(str(request)))
        # Make service call and store service response to check for success and number of waypoints transferred
        response = waypoint_push_client.call(request)
        self.get_logger().info(str(response))
        self.get_logger().info('Waypoint sent: %s .' %('successfully' if response.success else 'not successfully'))
def main(args=None):
    rclpy.init(args=args)
    waypoint_push = WaypointPushNode()
    rclpy.spin(waypoint_push)
    waypoint_push.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

which in terminal outputs:

[INFO] [1687170909.321930525] [waypoint_push_node]: Waypoint sent: mavros_msgs.srv.WaypointPush_Request(start_index=0, waypoints=[mavros_msgs.msg.Waypoint(frame=0, command=16, is_current=False, autocontinue=True, param1=0.0, param2=0.0, param3=0.0, param4=nan, x_lat=50.000000, y_long=10.0000, z_alt=0.0)]).

Expected behavior:

response = waypoint_push_client.call(request) according to /srv/WayPointPush should be a bool success and a uint32 wp_transfered which I would expect is the number of transfered waypoints?!

Anyway, there is no response from the Service, hence self.get_logger().info('Waypoint sent: %s .' %('successfully' if response.success else 'not successfully')) won't work. BUT the Mavros Node throws:

[INFO] [1687181434.292309661] [mavros.sys]: FCU: Flight plan received

Is this a bug? In parallel I have issues with MissionPlanner, to read Waypoints on my connected Windows Machine. I will come back to you once this works again.

MAVROS version and platform

Mavros: 2.0.0 ROS2 Alpha, installed with sudo apt install ros-foxy-mavros
ROS2: Foxy
Ubuntu: 20.04. LTS

Autopilot type and version

[X] ArduPilot
[ ] PX4

Version: 4.2.3. Ardurover

Details

ros2 run mavros mavros_node --ros-args --params-file ./mavros_param.yaml

with mavros_param.yaml:

# mavros_param.yaml
mavros:
  ros__parameters: {}

mavros_router:
  ros__parameters: {}

mavros_node:
  ros__parameters:
    # Parameters, see: http://wiki.ros.org/mavros
    fcu_url: /dev/ttyUSB0:57600
    system_id: 255
    component_id: 0
    target_system_id: 1
    target_component_id: 1

ros2 run mavros mav checkid returns Router topic: /uas1/mavlink_source, target: 1.1

[INFO] [1686747788.943812714] [mavros_node]: Starting mavros_node container
[INFO] [1686747788.943862778] [mavros_node]: FCU URL: /dev/ttyUSB0:57600
[INFO] [1686747788.943870539] [mavros_node]: GCS URL: 
[INFO] [1686747788.943875523] [mavros_node]: UAS Prefix: /uas1
[INFO] [1686747788.943879979] [mavros_node]: Starting mavros router node
[INFO] [1686747788.945723999] [mavros_router]: Built-in SIMD instructions: SSE, SSE2
[INFO] [1686747788.945737300] [mavros_router]: Built-in MAVLink package version: 2022.12.30
[INFO] [1686747788.945742762] [mavros_router]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all cubepilot development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[INFO] [1686747788.945748080] [mavros_router]: MAVROS Router started
[INFO] [1686747788.945765730] [mavros_router]: Requested to add endpoint: type: 0, url: /dev/ttyUSB0:57600
[INFO] [1686747788.945791745] [mavros_router]: Endpoint link[1000] created
[INFO] [1686747788.946507575] [mavros_router]: link[1000] opened successfully
[INFO] [1686747788.946532163] [mavros_router]: Requested to add endpoint: type: 2, url: /uas1
[INFO] [1686747788.946547918] [mavros_router]: Endpoint link[1001] created
[INFO] [1686747788.946842226] [mavros_router]: link[1001] opened successfully
[INFO] [1686747788.946861934] [mavros_node]: Starting mavros uas node
[INFO] [1686747788.970065458] [mavros]: UAS Executor started, threads: 16
[INFO] [1686747788.978579203] [mavros]: Plugin actuator_control created
[INFO] [1686747788.978611000] [mavros]: Plugin actuator_control initialized
[INFO] [1686747788.980122015] [mavros]: Plugin altitude created
[INFO] [1686747788.980141994] [mavros]: Plugin altitude initialized
[INFO] [1686747788.982914723] [mavros]: Plugin command created
[INFO] [1686747788.982936728] [mavros]: Plugin command initialized
[INFO] [1686747788.986646516] [mavros]: Plugin ftp created
[INFO] [1686747788.986686552] [mavros]: Plugin ftp initialized
[INFO] [1686747788.989798906] [mavros]: Plugin geofence created
[INFO] [1686747788.989839726] [mavros]: Plugin geofence initialized
[INFO] [1686747788.993609992] [mavros]: Plugin global_position created
[INFO] [1686747788.993636372] [mavros]: Plugin global_position initialized
[INFO] [1686747788.996030728] [mavros]: Plugin home_position created
[INFO] [1686747788.996050977] [mavros]: Plugin home_position initialized
[INFO] [1686747788.998918694] [mavros]: Plugin imu created
[INFO] [1686747788.998946517] [mavros]: Plugin imu initialized
[INFO] [1686747789.001616077] [mavros]: Plugin local_position created
[INFO] [1686747789.001637031] [mavros]: Plugin local_position initialized
[INFO] [1686747789.003767341] [mavros]: Plugin manual_control created
[INFO] [1686747789.003787339] [mavros]: Plugin manual_control initialized
[INFO] [1686747789.005928639] [mavros]: Plugin nav_controller_output created
[INFO] [1686747789.005953396] [mavros]: Plugin nav_controller_output initialized
[INFO] [1686747789.008767702] [mavros]: Plugin param created
[INFO] [1686747789.008789309] [mavros]: Plugin param initialized
[INFO] [1686747789.011634272] [mavros]: Plugin rallypoint created
[INFO] [1686747789.011663306] [mavros]: Plugin rallypoint initialized
[INFO] [1686747789.014271024] [mavros]: Plugin rc_io created
[INFO] [1686747789.014293218] [mavros]: Plugin rc_io initialized
[INFO] [1686747789.016693766] [mavros]: Plugin setpoint_accel created
[INFO] [1686747789.016709180] [mavros]: Plugin setpoint_accel initialized
[INFO] [1686747789.019456814] [mavros]: Plugin setpoint_attitude created
[INFO] [1686747789.019472422] [mavros]: Plugin setpoint_attitude initialized
[INFO] [1686747789.022641217] [mavros]: Plugin setpoint_position created
[INFO] [1686747789.022657561] [mavros]: Plugin setpoint_position initialized
[INFO] [1686747789.026150345] [mavros]: Plugin setpoint_raw created
[INFO] [1686747789.026180248] [mavros]: Plugin setpoint_raw initialized
[INFO] [1686747789.029445202] [mavros]: Plugin setpoint_trajectory created
[INFO] [1686747789.029461999] [mavros]: Plugin setpoint_trajectory initialized
[INFO] [1686747789.032458522] [mavros]: Plugin setpoint_velocity created
[INFO] [1686747789.032493519] [mavros]: Plugin setpoint_velocity initialized
[INFO] [1686747789.038280464] [mavros]: Plugin sys_status created
[INFO] [1686747789.038326916] [mavros]: Plugin sys_status initialized
[INFO] [1686747789.041205260] [mavros.time]: TM: Timesync mode: MAVLINK
[INFO] [1686747789.042182200] [mavros]: Plugin sys_time created
[INFO] [1686747789.042201453] [mavros]: Plugin sys_time initialized
[INFO] [1686747789.046780783] [mavros]: Plugin waypoint created
[INFO] [1686747789.046820154] [mavros]: Plugin waypoint initialized
[INFO] [1686747789.049996559] [mavros]: Plugin wind_estimation created
[INFO] [1686747789.050017667] [mavros]: Plugin wind_estimation initialized
[INFO] [1686747789.050444486] [mavros]: Built-in SIMD instructions: SSE, SSE2
[INFO] [1686747789.050455737] [mavros]: Built-in MAVLink package version: 2022.12.30
[INFO] [1686747789.050460945] [mavros]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all cubepilot development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[INFO] [1686747789.050466474] [mavros]: MAVROS UAS via /uas1 started. MY ID 1.191, TARGET ID 1.1
[INFO] [1686747789.079088826] [mavros_router]: link[1000] detected remote address 1.1
[INFO] [1686747789.103168523] [mavros.rc]: RC_CHANNELS message detected!
[INFO] [1686747789.110205923] [mavros.imu]: IMU: Raw IMU message used.
[WARN] [1686747789.110257164] [mavros.imu]: IMU: linear acceleration on RAW_IMU known on APM only.
[WARN] [1686747789.110264518] [mavros.imu]: IMU: ~imu/data_raw stores unscaled raw acceleration report.
[INFO] [1686747789.426658698] [mavros_router]: link[1001] detected remote address 1.191
[WARN] [1686747789.785522172] [mavros.rc]: RC override not supported by this FCU!
[INFO] [1686747789.900602146] [mavros]: CON: Got HEARTBEAT, connected. FCU: ArduPilot
[INFO] [1686747789.900853814] [mavros.mission]: WP: detected enable_partial_push: 1
[INFO] [1686747790.081365212] [mavros_router]: link[1000] detected remote address 51.68
[INFO] [1686747790.166584086] [mavros.rc]: RC_CHANNELS message detected!
[INFO] [1686747790.173665104] [mavros.imu]: IMU: Raw IMU message used.
[WARN] [1686747791.023949484] [mavros.cmd]: CMD: Unexpected command 520, result 0
[INFO] [1686747791.037923265] [mavros.geofence]: GF: Using MISSION_ITEM_INT
[INFO] [1686747791.038018838] [mavros.rallypoint]: RP: Using MISSION_ITEM_INT
[INFO] [1686747791.038058445] [mavros.mission]: WP: Using MISSION_ITEM_INT
[INFO] [1686747791.038107835] [mavros.sys]: VER: 1.1: Capabilities         0x000000000000f1ef
[INFO] [1686747791.038154734] [mavros.sys]: VER: 1.1: Flight software:     040203ff (2172cfb3)
[INFO] [1686747791.038186428] [mavros.sys]: VER: 1.1: Middleware software: 00000000 (        )
[INFO] [1686747791.038215658] [mavros.s@W.�)VER: 1.1: OS software:         00000000 (38022f4f
[INFO] [1686747791.038248202] [mavros.sys]: VER: 1.1: Board hardware:      00320000
[INFO] [1686747791.038284177] [mavros.sys]: VER: 1.1: VID/PID:             1209:5740
[INFO] [1686747791.038314457] [mavros.sys]: VER: 1.1: UID:                 0000000000000000
^C[INFO] [1686747796.868565087] [rclcpp]

Diagnostics

ros2 topic echo /diagnostics:

header:
  stamp:
    sec: 1686814416
    nanosec: 653634647
  frame_id: ''
status:
- level: "\0"
  name: 'mavros_router: MAVROS Router'
  message: ok
  hardware_id: none
  values:
  - key: Endpoints
    value: '2'
  - key: Messages routed
    value: '1846'
  - key: Messages sent
    value: '1846'
  - key: Messages dropped
    value: '0'
- level: "\0"
  name: 'mavros_router: endpoint 1000: /dev/ttyUSB0:57600'
  message: ok
  hardware_id: none
  values:
  - key: Received packets
    value: '1803'
  - key: Dropped packets
    value: '0'
  - key: Buffer overruns
    value: '0'
  - key: Parse errors
    value: '0'
  - key: Rx sequence number
    value: '91'
  - key: Tx sequence number
    value: '0'
  - key: Rx total bytes
    value: '60468'
  - key: Tx total bytes
    value: '916'
  - key: Rx speed
    value: inf
  - key: Tx speed
    value: inf
  - key: Remotes count
    value: '5'
  - key: Remote [0]
    value: '0.0'
  - key: Remote [1]
    value: '1.0'
  - key: Remote [2]
    value: '1.1'
  - key: Remote [3]
    value: '51.0'
  - key: Remote [4]
    value: '51.68'
- level: "\0"
  name: 'mavros_router: endpoint 1001: /uas1'
  message: ok
  hardware_id: none
  values:
  - key: Remotes count
    value: '3'
  - key: Remote [0]
    value: '0.0'
  - key: Remote [1]
    value: '1.0'
  - key: Remote [2]
    value: '1.191'
---
header:
  stamp:
    sec: 1686814416
    nanosec: 656804977
  frame_id: ''
status:
- level: "\0"
  name: 'mavros: MAVROS UAS'
  message: connected
  hardware_id: uas:///uas1
  values: []
- level: "\x02"
  name: 'mavros: GPS'
  message: No satellites
  hardware_id: uas:///uas1
  values:
  - key: Satellites visible
    value: '0'
  - key: Fix type
    value: '1'
  - key: EPH (m)
    value: '100.00'
  - key: EPV (m)
    value: '100.00'
- level: "\x02"
  name: 'mavros: System'
  message: Sensor health
  hardware_id: uas:///uas1
  values:
  - key: Sensor present
    value: '0x1330DC2F'
  - key: Sensor enabled
    value: '0x0120802F'
  - key: Sensor health
    value: '0x0310812F'
  - key: 3D gyro
    value: Ok
  - key: 3D accelerometer
    value: Ok
  - key: 3D magnetometer
    value: Ok
  - key: absolute pressure
    value: Ok
  - key: GPS
    value: Ok
  - key: motor outputs / control
    value: Ok
  - key: AHRS subsystem health
    value: Fail
  - key: Logging
    value: Ok
  - key: CPU Load (%)
    value: '12.8'
  - key: Drop rate (%)
    value: '0.0'
  - key: Errors comm
    value: '0'
  - key: 'Errors count #1'
    value: '0'
  - key: 'Errors count #2'
    value: '0'
  - key: 'Errors count #3'
    value: '0'
  - key: 'Errors count #4'
    value: '0'
- level: "\x01"
  name: 'mavros: Battery'
  message: Low voltage
  hardware_id: uas:///uas1
  values:
  - key: Voltage
    value: '0.00'
  - key: Current
    value: '-0.0'
  - key: Remaining
    value: '-1.0'
- level: "\0"
  name: 'mavros: Heartbeat'
  message: Normal
  hardware_id: uas:///uas1
  values:
  - key: Heartbeats since startup
    value: '32'
  - key: Frequency (Hz)
    value: '0.999998'
  - key: Vehicle type
    value: Ground rover
  - key: Autopilot type
    value: ArduPilot
  - key: Mode
    value: MANUAL
  - key: System status
    value: ACTIVE

From Source ros2 topic echo /uas1/mavlink_source:

header:
  stamp:
    sec: 1686818223
    nanosec: 568630184
  frame_id: ep:1000
framing_status: 1
magic: 253
len: 7
incompat_flags: 0
compat_flags: 0
seq: 104
sysid: 1
compid: 1
msgid: 152
checksum: 42716
payload64:
- 845941053521920
signature: []

and Sink ros2 topic echo /uas1/mavlink_source:

header:
  stamp:
    sec: 1686818316
    nanosec: 446609267
  frame_id: mavros
framing_status: 1
magic: 253
len: 9
incompat_flags: 0
compat_flags: 0
seq: 161
sysid: 1
compid: 191
msgid: 0
checksum: 7283
payload64:
- 342282445082591232
- 12206563605199221507
signature: []

ROS2 Services and Topics

ros2 topic list:

/diagnostics
/mavros/actuator_control
/mavros/altitude
/mavros/battery
/mavros/estimator_status
/mavros/extended_state
/mavros/geofence/fences
/mavros/global_position/compass_hdg
/mavros/global_position/global
/mavros/global_position/gp_lp_offset
/mavros/global_position/gp_origin
/mavros/global_position/local
/mavros/global_position/raw/fix
/mavros/global_position/raw/gps_vel
/mavros/global_position/raw/satellites
/mavros/global_position/rel_alt
/mavros/global_position/set_gp_origin
/mavros/home_position/home
/mavros/home_position/set
/mavros/imu/data
/mavros/imu/data_raw
/mavros/imu/diff_pressure
/mavros/imu/mag
/mavros/imu/static_pressure
/mavros/imu/temperature_baro
/mavros/imu/temperature_imu
/mavros/local_position/accel
/mavros/local_position/odom
/mavros/local_position/pose
/mavros/local_position/pose_cov
/mavros/local_position/velocity_body
/mavros/local_position/velocity_body_cov
/mavros/local_position/velocity_local
/mavros/manual_control/control
/mavros/manual_control/send
/mavros/mission/reached
/mavros/mission/waypoints
/mavros/nav_controller_output/output
/mavros/param/event
/mavros/rallypoint/rallypoints
/mavros/rc/in
/mavros/rc/out
/mavros/rc/override
/mavros/setpoint_accel/accel
/mavros/setpoint_attitude/cmd_vel
/mavros/setpoint_attitude/thrust
/mavros/setpoint_position/global
/mavros/setpoint_position/global_to_local
/mavros/setpoint_position/local
/mavros/setpoint_raw/attitude
/mavros/setpoint_raw/global
/mavros/setpoint_raw/local
/mavros/setpoint_raw/target_attitude
/mavros/setpoint_raw/target_global
/mavros/setpoint_raw/target_local
/mavros/setpoint_trajectory/desired
/mavros/setpoint_trajectory/local
/mavros/setpoint_velocity/cmd_vel
/mavros/setpoint_velocity/cmd_vel_unstamped
/mavros/state
/mavros/statustext/recv
/mavros/statustext/send
/mavros/target_actuator_control
/mavros/time_reference
/mavros/timesync_status
/mavros/wind_estimation
/parameter_events
/rosout
/tf
/tf_static
/uas1/mavlink_sink
/uas1/mavlink_source

ros2 service list:

/mavros/actuator_control/describe_parameters
/mavros/actuator_control/get_parameter_types
/mavros/actuator_control/get_parameters
/mavros/actuator_control/list_parameters
/mavros/actuator_control/set_parameters
/mavros/actuator_control/set_parameters_atomically
/mavros/altitude/describe_parameters
/mavros/altitude/get_parameter_types
/mavros/altitude/get_parameters
/mavros/altitude/list_parameters
/mavros/altitude/set_parameters
/mavros/altitude/set_parameters_atomically
/mavros/cmd/arming
/mavros/cmd/command
/mavros/cmd/command_int
/mavros/cmd/describe_parameters
/mavros/cmd/get_parameter_types
/mavros/cmd/get_parameters
/mavros/cmd/land
/mavros/cmd/list_parameters
/mavros/cmd/set_home
/mavros/cmd/set_parameters
/mavros/cmd/set_parameters_atomically
/mavros/cmd/takeoff
/mavros/cmd/trigger_control
/mavros/cmd/trigger_interval
/mavros/cmd/vtol_transition
/mavros/describe_parameters
/mavros/ftp/checksum
/mavros/ftp/close
/mavros/ftp/describe_parameters
/mavros/ftp/get_parameter_types
/mavros/ftp/get_parameters
/mavros/ftp/list
/mavros/ftp/list_parameters
/mavros/ftp/mkdir
/mavros/ftp/open
/mavros/ftp/read
/mavros/ftp/remove
/mavros/ftp/rename
/mavros/ftp/reset
/mavros/ftp/rmdir
/mavros/ftp/set_parameters
/mavros/ftp/set_parameters_atomically
/mavros/ftp/truncate
/mavros/ftp/write
/mavros/geofence/clear
/mavros/geofence/describe_parameters
/mavros/geofence/get_parameter_types
/mavros/geofence/get_parameters
/mavros/geofence/list_parameters
/mavros/geofence/pull
/mavros/geofence/push
/mavros/geofence/set_parameters
/mavros/geofence/set_parameters_atomically
/mavros/get_parameter_types
/mavros/get_parameters
/mavros/global_position/describe_parameters
/mavros/global_position/get_parameter_types
/mavros/global_position/get_parameters
/mavros/global_position/list_parameters
/mavros/global_position/set_parameters
/mavros/global_position/set_parameters_atomically
/mavros/home_position/describe_parameters
/mavros/home_position/get_parameter_types
/mavros/home_position/get_parameters
/mavros/home_position/list_parameters
/mavros/home_position/req_update
/mavros/home_position/set_parameters
/mavros/home_position/set_parameters_atomically
/mavros/imu/describe_parameters
/mavros/imu/get_parameter_types
/mavros/imu/get_parameters
/mavros/imu/list_parameters
/mavros/imu/set_parameters
/mavros/imu/set_parameters_atomically
/mavros/list_parameters
/mavros/local_position/describe_parameters
/mavros/local_position/get_parameter_types
/mavros/local_position/get_parameters
/mavros/local_position/list_parameters
/mavros/local_position/set_parameters
/mavros/local_position/set_parameters_atomically
/mavros/manual_control/describe_parameters
/mavros/manual_control/get_parameter_types
/mavros/manual_control/get_parameters
/mavros/manual_control/list_parameters
/mavros/manual_control/set_parameters
/mavros/manual_control/set_parameters_atomically
/mavros/mission/clear
/mavros/mission/describe_parameters
/mavros/mission/get_parameter_types
/mavros/mission/get_parameters
/mavros/mission/list_parameters
/mavros/mission/pull
/mavros/mission/push
/mavros/mission/set_current
/mavros/mission/set_parameters
/mavros/mission/set_parameters_atomically
/mavros/nav_controller_output/describe_parameters
/mavros/nav_controller_output/get_parameter_types
/mavros/nav_controller_output/get_parameters
/mavros/nav_controller_output/list_parameters
/mavros/nav_controller_output/set_parameters
/mavros/nav_controller_output/set_parameters_atomically
/mavros/param/describe_parameters
/mavros/param/get_parameter_types
/mavros/param/get_parameters
/mavros/param/list_parameters
/mavros/param/pull
/mavros/param/set
/mavros/param/set_parameters
/mavros/param/set_parameters_atomically
/mavros/rallypoint/clear
/mavros/rallypoint/describe_parameters
/mavros/rallypoint/get_parameter_types
/mavros/rallypoint/get_parameters
/mavros/rallypoint/list_parameters
/mavros/rallypoint/pull
/mavros/rallypoint/push
/mavros/rallypoint/set_parameters
/mavros/rallypoint/set_parameters_atomically
/mavros/rc/describe_parameters
/mavros/rc/get_parameter_types
/mavros/rc/get_parameters
/mavros/rc/list_parameters
/mavros/rc/set_parameters
/mavros/rc/set_parameters_atomically
/mavros/set_message_interval
/mavros/set_mode
/mavros/set_parameters
/mavros/set_parameters_atomically
/mavros/set_stream_rate
/mavros/setpoint_accel/describe_parameters
/mavros/setpoint_accel/get_parameter_types
/mavros/setpoint_accel/get_parameters
/mavros/setpoint_accel/list_parameters
/mavros/setpoint_accel/set_parameters
/mavros/setpoint_accel/set_parameters_atomically
/mavros/setpoint_attitude/describe_parameters
/mavros/setpoint_attitude/get_parameter_types
/mavros/setpoint_attitude/get_parameters
/mavros/setpoint_attitude/list_parameters
/mavros/setpoint_attitude/set_parameters
/mavros/setpoint_attitude/set_parameters_atomically
/mavros/setpoint_position/describe_parameters
/mavros/setpoint_position/get_parameter_types
/mavros/setpoint_position/get_parameters
/mavros/setpoint_position/list_parameters
/mavros/setpoint_position/set_parameters
/mavros/setpoint_position/set_parameters_atomically
/mavros/setpoint_raw/describe_parameters
/mavros/setpoint_raw/get_parameter_types
/mavros/setpoint_raw/get_parameters
/mavros/setpoint_raw/list_parameters
/mavros/setpoint_raw/set_parameters
/mavros/setpoint_raw/set_parameters_atomically
/mavros/setpoint_trajectory/describe_parameters
/mavros/setpoint_trajectory/get_parameter_types
/mavros/setpoint_trajectory/get_parameters
/mavros/setpoint_trajectory/list_parameters
/mavros/setpoint_trajectory/reset
/mavros/setpoint_trajectory/set_parameters
/mavros/setpoint_trajectory/set_parameters_atomically
/mavros/setpoint_velocity/describe_parameters
/mavros/setpoint_velocity/get_parameter_types
/mavros/setpoint_velocity/get_parameters
/mavros/setpoint_velocity/list_parameters
/mavros/setpoint_velocity/set_parameters
/mavros/setpoint_velocity/set_parameters_atomically
/mavros/sys/describe_parameters
/mavros/sys/get_parameter_types
/mavros/sys/get_parameters
/mavros/sys/list_parameters
/mavros/sys/set_parameters
/mavros/sys/set_parameters_atomically
/mavros/time/describe_parameters
/mavros/time/get_parameter_types
/mavros/time/get_parameters
/mavros/time/list_parameters
/mavros/time/set_parameters
/mavros/time/set_parameters_atomically
/mavros/vehicle_info_get
/mavros/wind/describe_parameters
/mavros/wind/get_parameter_types
/mavros/wind/get_parameters
/mavros/wind/list_parameters
/mavros/wind/set_parameters
/mavros/wind/set_parameters_atomically
/mavros_node/describe_parameters
/mavros_node/get_parameter_types
/mavros_node/get_parameters
/mavros_node/list_parameters
/mavros_node/set_parameters
/mavros_node/set_parameters_atomically
/mavros_router/add_endpoint
/mavros_router/del_endpoint
/mavros_router/describe_parameters
/mavros_router/get_parameter_types
/mavros_router/get_parameters
/mavros_router/list_parameters
/mavros_router/set_parameters
/mavros_router/set_parameters_atomically

[vooon]

Sounds familiar to the issue somewhere inside rcl of Foxy that some messages not being received (rpc is affected).

Could you please try to build latest version from source? 2.0.0 quite old.
Also i suggest to upgrade ros distro, Foxy is EOLed.

Have you tried mav wp pull or mav wp push?
https://github.com/mavlink/mavros/blob/ros2/mavros/mavros/cmd/mission.py#L279

Note it uses python helper client library.

[Scoeerg]

Thanks @vooon Unfortunately ROS2 Foxy needs to stay, since some sensor manufacturers have not yet published stable ros2 humble nodes for their hardware, even though it's EOL.

1. I now built from source (current ros2 branch, version 2.5.0)
2. same python-code, see above. Pushing WaypointList to topic /mavros/mission/push
3. From what I understand the ros2 equivalent to mav wp pull/push is the python code above, i.e. a service call to /mavros/mission/push

To check if this was successful, looking into MissionPlanner and try to read from Pixhawk.

a) MissionPlanner throws error Timeout on Read-GetWaypointCount
b) The response = waypoint_push_client.call(request) still is not received, see above
c) I can finally read the waypoints, by ros2 topic echo /mavros/mission/waypoints which responds with:

current_seq: 0
waypoints:
- frame: 0
  command: 16
  is_current: false
  autocontinue: true
  param1: 0.0
  param2: 0.0
  param3: 0.0
  param4: .nan
  x_lat: 50.0000
  y_long: 7.0000
  z_alt: 0.0
---
current_seq: 0
waypoints:
- frame: 0
  command: 16
  is_current: false
  autocontinue: true
  param1: 0.0
  param2: 0.0
  param3: 0.0
  param4: 0.0
  x_lat: XXX
  y_long: XXX
  z_alt: XXX
---

where XXX is roughly the current GPS position (home position?). I would expect the sent position will be saved on Pixhawk. But once I restart the mavros-node and repeat ros2 topic echo /mavros/mission/waypoints I get

current_seq: 0
waypoints:
- frame: 0
  command: 16
  is_current: false
  autocontinue: true
  param1: 0.0
  param2: 0.0
  param3: 0.0
  param4: 0.0
  x_lat: YYY
  y_long: YYY
  z_alt: YYY
---

where YYY is not equal XXX, hence the waypoint I sent earlier does not seem to persist.

[Scoeerg]

I updated the python-code. It is now a little cleaner and I added request.start_index = 0. Here:

# see https://github.com/mavlink/mavros/issues/1718
# and https://github.com/mavlink/mavros/issues/1867
import rclpy
from rclpy.node import Node
from mavros_msgs.srv import WaypointPush # http://docs.ros.org/en/noetic/api/mavros_msgs/html/srv/WaypointPush.html
from mavros_msgs.msg import Waypoint # http://docs.ros.org/en/hydro/api/mavros/html/msg/Waypoint.html

class WaypointPushNode(Node):
    def __init__(self):
        super().__init__('waypoint_push_node')
        waypoint_push_client = self.create_client(WaypointPush, '/mavros/mission/push')
        # Waypoint 1:
        waypoint_1 = Waypoint()
        waypoint_1.frame = 0
        waypoint_1.command = 16
        waypoint_1.is_current = False
        waypoint_1.autocontinue = True
        waypoint_1.param1 = float(0)
        waypoint_1.param2 = float(0)
        waypoint_1.param3 = float(0)
        waypoint_1.param4 = float("nan")
        waypoint_1.x_lat = float(50.0000)
        waypoint_1.y_long = float(7.0000)
        waypoint_1.z_alt = float(0)
        # Waypoint 2:
        waypoint_2 = Waypoint()
        waypoint_2.frame = 0
        waypoint_2.command = 16
        waypoint_2.is_current = False
        waypoint_2.autocontinue = True
        waypoint_2.param1 = float(0)
        waypoint_2.param2 = float(0)
        waypoint_2.param3 = float(0)
        waypoint_2.param4 = float("nan")
        waypoint_2.x_lat = float(51.0000)
        waypoint_2.y_long = float(8.0000)
        waypoint_2.z_alt = float(0.0)
        # Populate your request
        request = WaypointPush.Request()
        request.start_index = 0
        request.waypoints.append(waypoint_1)
        request.waypoints.append(waypoint_2)
        self.get_logger().info('Waypoint sending: %s.' %(str(request)))
        response = waypoint_push_client.call(request)
        self.get_logger().info('Waypoint sent: %s.' %('successfully' if response.success else 'not successfully'))
def main(args=None):
    rclpy.init(args=args)
    waypoint_push = WaypointPushNode()
    rclpy.spin(waypoint_push)
    waypoint_push.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

to which the mavros_node in terminal responds with

[INFO] [1687257439.368765437] [mavros.mission]: WP: item #0  F:0 C: 16 p: 0 0 0 0 x: XXX y: XXX z: XXX
[INFO] [1687257439.684444558] [mavros.mission]: WP: item #1  F:0 C: 16 p: 0 0 0 0 x: 51.0000 y: 8.0000 z: 0

where XXX is home position. Interestingly, it seems to take the second waypoint but not the first. [EDIT: Confirmed, sending 3 waypoints, it accepts waypoints 2 and 3. Confirmed with ros2 topic echo /mavros/mission/waypoints.] Changing the request.start_index to 1 (not overwriting first mission waypoint home position?!) throws a warning inside mavros_node [WARN] [1687257279.555358071] [mavros.mission]: WP: Partial push out of range rejected..

Good news though, waypoints are now persistent, hence restarting mavros_node and ros2 topic echo /mavros/mission/waypoints returns the waypoints previously sent. Must have been a user error I cannot duplicate.

tl;dr:
response-error is persistent.
ignore-first-waypoint-error is persistent
waypoints-not-persistent-error is solved (somehow 🥇)

[vooon]

Ardupilot uses first waypoint to store home position (position captured on arming). That's why mav wp have some quirks for that.

[Scoeerg]

Hi. Hold on tight, I will close this issue with this comment.

A couple things concerning waypoints:

1. waypoints are very badly named, but that is Ardupilot/Mavlink convention, not Mavros. Waypoints CAN be waypoints in the sense that they mean (GPS) points in some reference frame. A waypoint will be e.g.

waypoint_2 = Waypoint()
        waypoint_2.frame = 0
        waypoint_2.command = 16
        waypoint_2.is_current = False
        waypoint_2.autocontinue = True
        waypoint_2.param1 = float(0)
        waypoint_2.param2 = float(0)
        waypoint_2.param3 = float(0)
        waypoint_2.param4 = float("nan")
        waypoint_2.x_lat = float(50.0000)
        waypoint_2.y_long = float(7.0000)
        waypoint_2.z_alt = float(0.0)

waypoint_2.command = 16 says this waypoint concerns actual positions. see waypoint_2.x_lat = float(50.0000) ... BUT another waypoint:

# waypoint 1:
        waypoint_1 = Waypoint()
        waypoint_1.frame = 0
        waypoint_1.command = 178
        waypoint_1.is_current = False
        waypoint_1.autocontinue = True
        waypoint_1.param1 = float(0)
        waypoint_1.param2 = float(0.35) #meters per second
        waypoint_1.param3 = float(0.0) #throttle percentage 0.0 to 1.0 THIS XOR PARAM3
        waypoint_1.param4 = float(0.0)
        waypoint_1.x_lat = float(0.0)
        waypoint_1.y_long = float(0.0)
        waypoint_1.z_alt = float(0.0)

where waypoint_1.command = 178 says this waypoint concerns velocity rather than position. see waypoint_1.param2 = float(0.35) ....
So waypoints would better be described as missionpoints, because they will define the way the robot/drone ... behaves during a mission. To understand this, I used a mixture of MissionPlanner and Ardupilot documentation.

2. as @vooon said:

Ardupilot uses first waypoint to store home position (position captured on arming). That's why mav wp have some quirks for that.

To "solve" this issue I simply hacked it together in a way that I will send a dummy waypoint waypoint_0as the first waypoint to be ignored. And only from the second waypoint waypoint_1 on I actually send the mission I want.

3. As usual, the problem was not the code but the idiot (me) trying to use it. This time, I copied some code from this mavros issue (see the opening of this issue), and it was not working. Reading the ros2 tutorials on Services in python again, combined with the remarks 1) and 2) then comes:

# see https://github.com/mavlink/mavros/issues/1718
# and https://github.com/mavlink/mavros/issues/1867
import rclpy
from rclpy.node import Node
from mavros_msgs.srv import WaypointPush # http://docs.ros.org/en/noetic/api/mavros_msgs/html/srv/WaypointPush.html
from mavros_msgs.msg import Waypoint # http://docs.ros.org/en/hydro/api/mavros/html/msg/Waypoint.html


class WaypointPushNode(Node):

    def __init__(self):
        super().__init__('waypointpush_node')
        self.client = self.create_client(WaypointPush, '/mavros/mission/push')
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Waypoint Push service is not available. Looking for /mavros/mission/push')
        # waypoints
        # waypoint 0: to be ignored
        waypoint_0 = Waypoint()
        waypoint_0.frame = 0
        waypoint_0.command = 16
        waypoint_0.is_current = False
        waypoint_0.autocontinue = True
        waypoint_0.param1 = float(0)
        waypoint_0.param2 = float(0)
        waypoint_0.param3 = float(0)
        waypoint_0.param4 = float("nan")
        waypoint_0.x_lat = float(49.00000)
        waypoint_0.y_long = float(7.00000)
        waypoint_0.z_alt = float(0)
        # waypoint 1: set the mission velocity
        waypoint_1 = Waypoint()
        waypoint_1.frame = 0
        waypoint_1.command = 178
        waypoint_1.is_current = False
        waypoint_1.autocontinue = True
        waypoint_1.param1 = float(0)
        waypoint_1.param2 = float(0.35) #meters per second
        waypoint_1.param3 = float(0.0) #throttle percentage 0.0 to 1.0 THIS XOR PARAM3
        waypoint_1.param4 = float(0.0)
        waypoint_1.x_lat = float(0.0)
        waypoint_1.y_long = float(0.0)
        waypoint_1.z_alt = float(0.0)
        #Waypoint 2: first actual position to drive to
        waypoint_2 = Waypoint()
        waypoint_2.frame = 0
        waypoint_2.command = 16
        waypoint_2.is_current = False
        waypoint_2.autocontinue = True
        waypoint_2.param1 = float(0)
        waypoint_2.param2 = float(0)
        waypoint_2.param3 = float(0)
        waypoint_2.param4 = float("nan")
        waypoint_2.x_lat = float(49.22222)
        waypoint_2.y_long = float(7.22222)
        waypoint_2.z_alt = float(0.0)
        # Waypoint 3: second actual position to drive to
        waypoint_3 = Waypoint()
        waypoint_3.frame = 0
        waypoint_3.command = 16
        waypoint_3.is_current = False
        waypoint_3.autocontinue = True
        waypoint_3.param1 = float(0)
        waypoint_3.param2 = float(0)
        waypoint_3.param3 = float(0)
        waypoint_3.param4 = float("nan")
        waypoint_3.x_lat = float(49.33333)
        waypoint_3.y_long = float(7.33333)
        waypoint_3.z_alt = float(0.0)
        # Waypoint 4: third actual position to drive to
        waypoint_4 = Waypoint()
        waypoint_4.frame = 0
        waypoint_4.command = 16
        waypoint_4.is_current = False
        waypoint_4.autocontinue = True
        waypoint_4.param1 = float(0)
        waypoint_4.param2 = float(0)
        waypoint_4.param3 = float(0)
        waypoint_4.param4 = float("nan")
        waypoint_4.x_lat = float(49.44444)
        waypoint_4.y_long = float(7.44444)
        waypoint_4.z_alt = float(0.0)
        self.waypointlist = [waypoint_0, waypoint_1, waypoint_2, waypoint_3, waypoint_4]
        # Build request
        self.request = WaypointPush.Request()
        self.request.start_index = 0
        for waypoint in self.waypointlist:
            self.request.waypoints.append(waypoint)

    def send_request(self):
        self.get_logger().info('Waypoint sent %s.' %(str(self.waypointlist)))
        self.future = self.client.call_async(self.request)
        rclpy.spin_until_future_complete(self, self.future)
        return self.future.result()


def main(args=None):
    rclpy.init(args=args)

    waypointpush_client = WaypointPushNode()
    response = waypointpush_client.send_request()
    waypointpush_client.get_logger().info('Waypoint sent %s.' %('successfully' if response.success else 'not successfully'))
    waypointpush_client.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

which then solves

tl;dr:

response-error is fixed - wrong use of ros2 Service
ignore-first-waypoint-error is fixed - explained above, Ardupilot does not overwrite first waypoint but says it's home position (first position recorded when arming)