mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
847 stars 983 forks source link

MAVROS2: RC-Override in ROS2 Foxy - Equivalent of `rosrun mavros mavparam set SYSID_MYGCS 1`? #1866

Closed Scoeerg closed 1 year ago

Scoeerg commented 1 year ago

This is only bug and feature tracker, please use it to report bugs or request features.


Issue details

I am publishing RCOVERRIDE in topic /mavros/rc/override using some minimal ROS2 Foxy Python Publisher. While the RC works fine, Rcoverride cannot engage the motors.

What I suspect is, that it's about SYSID_MYGCS which is 1 for the Pixhawk, but supposedly must also be 1 for the companion computer. In ROS1 the command would be something along the line of rosrun mavros mavparam set SYSID_MYGCS 1. What's the equivalent in ROS2? I would guess it's system_id, component_id, target_system_id or target_component_id in the parameter file.

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: []

and for those who wonder, this is the minimal publisher.py:

import rclpy
from rclpy.node import Node
from mavros_msgs.msg import OverrideRCIn

class RCOverride(Node):

    def __init__(self):
        super().__init__('rcoverride_publisher')
        self.publisher_ = self.create_publisher(OverrideRCIn, '/mavros/rc/override', 10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)

    def timer_callback(self):
        msg = OverrideRCIn()
        msg.channels = [1800, 0, 1800, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        self.publisher_.publish(msg)
        self.get_logger().info('Type: %s' %(type(msg.channels)))
        self.get_logger().info('Published RC-Override to Pixhawk using MavRos: %s' %(" ".join([str(x) for x in msg.channels])))

def main(args=None):
    rclpy.init(args=args)
    publisher = RCOverride()
    rclpy.spin(publisher)
    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Which might be an issue in itself. Since Python is not a typed language, using MissionPlanner to observe the Ardupilot tells me my msg.channels = [1800, 0, 1800, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] has type array uint16 but the RC-Message has type list(?). Probably nothing, if interested I can attach some screenshots.

vooon commented 1 year ago

ros run mavros mav param --help or ros param list /mavros/param --help (a bit unsure about ros command).

vooon commented 1 year ago

Most of the commands mav<something> became mav <something>, a subcommand to mav.

E.g.: https://github.com/mavlink/mavros/blob/ros2/mavros/mavros/cmd/param.py

Scoeerg commented 1 year ago

Thanks @vooon

ros2 param list /mavros/param is a dump from all parameters one can set, i.e. a VERY long list:

  ACRO_TURN_RATE
  .
  .
  .
  WRC_ENABLE

one of which is actually SYSID_MYGCS. Then running based on ROS2 Tutorial on Parameters

ros2 param set /mavros/param SYSID_MYGCS 1

worked like a charm. Will close the issue.