search

mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
902 stars 993 forks source link

RC override publisher doesn't working #1632

Open esharet opened 3 years ago

esharet commented 3 years ago

Issue details

When publishing to /mavros/rc/override topic, we don't get any change for topics /mavros/rc/in or /mavros/rc/out. Everything look fine, we managed to arm the vehicle through service and to change its mode but the vehicle's servos do not change. The vehicle is Rover (SITL) and we tried different modes (MANUAL, GUIDED). Does anyone know how to fix this please?

launch params:

mavros:
  ros__parameters: {}
mavros_router: {}

mavros_node:
  ros__parameters:
    fcu_url: udp://:14550@
    gcs_url: tcp-l://:5670
    tgt_system: 1
    tgt_component : 1

rc_override_publisher.py:

import rclpy
from rclpy.node import Node
import time
from math import *

from mavros.utils import *
from mavros_msgs.msg import OverrideRCIn, RCOut, State

class My_MavRover(Node):
    def __init__(self):
        super().__init__('rc_over_pub')
        self.rc_override_pub = self.create_publisher(OverrideRCIn, '/mavros/rc/override', 10)
        self.do_rc_override()

    def do_rc_override(self,):
        while(1):
            rc_msg = OverrideRCIn()

            time.sleep(0.5)
            rc_msg.channels = [1800,1500,1800,1500,0, 0, 0, 0]
            self.get_logger().info("rc_msg to pub: %s" % (rc_msg.channels))
            self.rc_override_pub.publish(rc_msg)

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

    minimal_mavrover = My_MavRover()

    rclpy.spin(minimal_mavrover)
    minimal_mavrover.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

ros2 topic echo /mavros/rc/out:

header:
  stamp:
    sec: 1633958085
    nanosec: 132588910
  frame_id: ''
channels:
- 1500
- 0
- 1500
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0

ros2 topic echo /mavros/rc/in:

header:
  stamp:
    sec: 1633958123
    nanosec: 630734455
  frame_id: ''
rssi: 0
channels:
- 1500
- 1500
- 1500
- 1500
- 1800
- 1000
- 1000
- 1800
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0

MAVROS version and platform

Mavros: 2.0.3 ROS: foxy Ubuntu: 20.04

Autopilot type and version

[V] ArduPilot [ ] PX4

Version: 4.1.0

Node logs

[INFO] [launch]: All log files can be found below /home/user/.ros/log/2021-10-11-15-47-33-756648-dev-14-90685
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [mavros_node-1]: process started with pid [90687]
[INFO] [main-2]: process started with pid [90689]
[mavros_node-1] [INFO] [1633956453.837119088] [mavros_node]: Starting mavros_node container
[mavros_node-1] [INFO] [1633956453.837175616] [mavros_node]: FCU URL: udp://:14550@
[mavros_node-1] [INFO] [1633956453.837185268] [mavros_node]: GCS URL: tcp-l://:5670
[mavros_node-1] [INFO] [1633956453.837189719] [mavros_node]: UAS Prefix: /uas1
[mavros_node-1] [INFO] [1633956453.837193619] [mavros_node]: Starting mavros router node
[mavros_node-1] [INFO] [1633956453.839607398] [mavros_router]: Built-in SIMD instructions: SSE, SSE2
[mavros_node-1] [INFO] [1633956453.839628332] [mavros_router]: Built-in MAVLink package version: 2021.9.9
[mavros_node-1] [INFO] [1633956453.839634838] [mavros_router]: Known MAVLink dialects: common ardupilotmega ASLUAV all development icarous matrixpilot paparazzi standard uAvionix ualberta
[mavros_node-1] [INFO] [1633956453.839641319] [mavros_router]: MAVROS Router started
[mavros_node-1] [INFO] [1633956453.839713494] [mavros_router]: Requested to add endpoint: type: 0, url: udp://:14550@
[mavros_node-1] [INFO] [1633956453.839882987] [mavros_router]: Endpoint link[1000] created
[mavros_node-1] [INFO] [1633956453.840728750] [mavros_router]: link[1000] opened successfully
[mavros_node-1] [INFO] [1633956453.840784910] [mavros_router]: Requested to add endpoint: type: 1, url: tcp-l://:5670
[mavros_node-1] [INFO] [1633956453.840822862] [mavros_router]: Endpoint link[1001] created
[mavros_node-1] [INFO] [1633956453.840989665] [mavros_router]: link[1001] opened successfully
[mavros_node-1] [INFO] [1633956453.841021736] [mavros_router]: Requested to add endpoint: type: 2, url: /uas1
[mavros_node-1] [INFO] [1633956453.841073780] [mavros_router]: Endpoint link[1002] created
[mavros_node-1] [INFO] [1633956453.841828167] [mavros_router]: link[1002] opened successfully
[mavros_node-1] [INFO] [1633956453.841862269] [mavros_node]: Starting mavros uas node
[mavros_node-1] [INFO] [1633956453.959723786] [mavros]: UAS Executor started
[mavros_node-1] [INFO] [1633956453.972322143] [mavros]: Plugin actuator_control created
[mavros_node-1] [INFO] [1633956453.972622470] [mavros]: Plugin actuator_control added to executor
[mavros_node-1] [INFO] [1633956453.972644775] [mavros]: Plugin actuator_control initialized
[mavros_node-1] [INFO] [1633956453.975351384] [mavros]: Plugin altitude created
[mavros_node-1] [INFO] [1633956453.975486291] [mavros]: Plugin altitude added to executor
[mavros_node-1] [INFO] [1633956453.975499337] [mavros]: Plugin altitude initialized
[mavros_node-1] [INFO] [1633956453.980551344] [mavros]: Plugin command created
[mavros_node-1] [INFO] [1633956453.980662642] [mavros]: Plugin command added to executor
[mavros_node-1] [INFO] [1633956453.980676244] [mavros]: Plugin command initialized
[mavros_node-1] [INFO] [1633956453.987234423] [mavros]: Plugin ftp created
[mavros_node-1] [INFO] [1633956453.987366177] [mavros]: Plugin ftp added to executor
[mavros_node-1] [INFO] [1633956453.987380698] [mavros]: Plugin ftp initialized
[mavros_node-1] [INFO] [1633956453.991866279] [mavros]: Plugin geofence created
[mavros_node-1] [INFO] [1633956453.992298838] [mavros]: Plugin geofence added to executor
[mavros_node-1] [INFO] [1633956453.992324679] [mavros]: Plugin geofence initialized
[mavros_node-1] [INFO] [1633956454.002540420] [mavros]: Plugin global_position created
[mavros_node-1] [INFO] [1633956454.002873942] [mavros]: Plugin global_position added to executor
[mavros_node-1] [INFO] [1633956454.002903497] [mavros]: Plugin global_position initialized
[mavros_node-1] [INFO] [1633956454.003298213] [mavros_router]: link[1000] detected remote address 1.1
[mavros_node-1] [INFO] [1633956454.008508782] [mavros]: Plugin home_position created
[mavros_node-1] [INFO] [1633956454.008657347] [mavros]: Plugin home_position added to executor
[mavros_node-1] [INFO] [1633956454.008679702] [mavros]: Plugin home_position initialized
[mavros_node-1] [INFO] [1633956454.019093050] [mavros]: Plugin imu created
[mavros_node-1] [INFO] [1633956454.019428340] [mavros]: Plugin imu added to executor
[mavros_node-1] [INFO] [1633956454.019447296] [mavros]: Plugin imu initialized
[mavros_node-1] [INFO] [1633956454.030276388] [mavros]: Plugin local_position created
[mavros_node-1] [INFO] [1633956454.030442416] [mavros]: Plugin local_position added to executor
[mavros_node-1] [INFO] [1633956454.030457633] [mavros]: Plugin local_position initialized
[mavros_node-1] [INFO] [1633956454.039704036] [mavros]: Plugin manual_control created
[mavros_node-1] [INFO] [1633956454.039825774] [mavros]: Plugin manual_control added to executor
[mavros_node-1] [INFO] [1633956454.039839567] [mavros]: Plugin manual_control initialized
[mavros_node-1] [INFO] [1633956454.044833788] [mavros]: Plugin param created
[mavros_node-1] [INFO] [1633956454.044953765] [mavros]: Plugin param added to executor
[mavros_node-1] [INFO] [1633956454.044967832] [mavros]: Plugin param initialized
[mavros_node-1] [INFO] [1633956454.055462729] [mavros]: Plugin rallypoint created
[mavros_node-1] [INFO] [1633956454.055620860] [mavros]: Plugin rallypoint added to executor
[mavros_node-1] [INFO] [1633956454.055637929] [mavros]: Plugin rallypoint initialized
[mavros_node-1] [INFO] [1633956454.065885530] [mavros]: Plugin rc_io created
[mavros_node-1] [INFO] [1633956454.066077997] [mavros]: Plugin rc_io added to executor
[mavros_node-1] [INFO] [1633956454.066092902] [mavros]: Plugin rc_io initialized
[mavros_node-1] [INFO] [1633956454.080316940] [mavros]: Plugin setpoint_accel created
[mavros_node-1] [INFO] [1633956454.080349761] [mavros]: Plugin setpoint_accel added to executor
[mavros_node-1] [INFO] [1633956454.080360034] [mavros]: Plugin setpoint_accel initialized
[mavros_node-1] [INFO] [1633956454.091820748] [mavros]: Plugin setpoint_attitude created
[mavros_node-1] [INFO] [1633956454.091852265] [mavros]: Plugin setpoint_attitude added to executor
[mavros_node-1] [INFO] [1633956454.091862099] [mavros]: Plugin setpoint_attitude initialized
[mavros_node-1] [INFO] [1633956454.103435379] [mavros]: Plugin setpoint_position created
[mavros_node-1] [INFO] [1633956454.103467381] [mavros]: Plugin setpoint_position added to executor
[mavros_node-1] [INFO] [1633956454.103478068] [mavros]: Plugin setpoint_position initialized
[mavros_node-1] [INFO] [1633956454.116223575] [mavros]: Plugin setpoint_raw created
[mavros_node-1] [INFO] [1633956454.116429147] [mavros]: Plugin setpoint_raw added to executor
[mavros_node-1] [INFO] [1633956454.116445356] [mavros]: Plugin setpoint_raw initialized
[mavros_node-1] [INFO] [1633956454.133313401] [mavros]: Plugin setpoint_trajectory created
[mavros_node-1] [INFO] [1633956454.133345426] [mavros]: Plugin setpoint_trajectory added to executor
[mavros_node-1] [INFO] [1633956454.133356229] [mavros]: Plugin setpoint_trajectory initialized
[mavros_node-1] [INFO] [1633956454.144415968] [mavros]: Plugin setpoint_velocity created
[mavros_node-1] [INFO] [1633956454.144451264] [mavros]: Plugin setpoint_velocity added to executor
[mavros_node-1] [INFO] [1633956454.144461991] [mavros]: Plugin setpoint_velocity initialized
[mavros_node-1] [INFO] [1633956454.166594359] [mavros]: Plugin sys_status created
[mavros_node-1] [INFO] [1633956454.167107870] [mavros]: Plugin sys_status added to executor
[mavros_node-1] [INFO] [1633956454.167136530] [mavros]: Plugin sys_status initialized
[mavros_node-1] [INFO] [1633956454.177744805] [mavros.time]: TM: Timesync mode: MAVLINK
[mavros_node-1] [INFO] [1633956454.179647468] [mavros]: Plugin sys_time created
[mavros_node-1] [INFO] [1633956454.179811893] [mavros]: Plugin sys_time added to executor
[mavros_node-1] [INFO] [1633956454.179829110] [mavros]: Plugin sys_time initialized
[mavros_node-1] [INFO] [1633956454.198841420] [mavros]: Plugin waypoint created
[mavros_node-1] [INFO] [1633956454.199070334] [mavros]: Plugin waypoint added to executor
[mavros_node-1] [INFO] [1633956454.199091203] [mavros]: Plugin waypoint initialized
[mavros_node-1] [INFO] [1633956454.211665158] [mavros]: Plugin wind_estimation created
[mavros_node-1] [INFO] [1633956454.211896117] [mavros]: Plugin wind_estimation added to executor
[mavros_node-1] [INFO] [1633956454.211917489] [mavros]: Plugin wind_estimation initialized
[mavros_node-1] [INFO] [1633956454.219022858] [mavros]: Built-in SIMD instructions: SSE, SSE2
[mavros_node-1] [INFO] [1633956454.219044812] [mavros]: Built-in MAVLink package version: 2021.9.9
[mavros_node-1] [INFO] [1633956454.219051389] [mavros]: Known MAVLink dialects: common ardupilotmega ASLUAV all development icarous matrixpilot paparazzi standard uAvionix ualberta
[mavros_node-1] [INFO] [1633956454.219056587] [mavros]: MAVROS UAS via /uas1 started. MY ID 1.191, TARGET ID 1.1
[mavros_node-1] [INFO] [1633956454.265840032] [mavros]: CON: Got HEARTBEAT, connected. FCU: ArduPilot
[mavros_node-1] [INFO] [1633956454.265966711] [mavros.mission]: WP: detected enable_partial_push: 1
[mavros_node-1] [INFO] [1633956454.267644815] [mavros.rc]: RC_CHANNELS message detected!
[mavros_node-1] [INFO] [1633956454.267770966] [mavros.imu]: IMU: Raw IMU message used.
[mavros_node-1] [INFO] [1633956455.156045358] [mavros_router]: link[1002] detected remote address 1.191
[mavros_node-1] [WARN] [1633956455.283857117] [mavros.cmd]: CMD: Unexpected command 520, result 0
[mavros_node-1] [INFO] [1633956455.284047799] [mavros.geofence]: GF: Using MISSION_ITEM_INT
[mavros_node-1] [INFO] [1633956455.284068774] [mavros.rallypoint]: RP: Using MISSION_ITEM_INT
[mavros_node-1] [INFO] [1633956455.284079095] [mavros.mission]: WP: Using MISSION_ITEM_INT
[mavros_node-1] [INFO] [1633956455.284092943] [mavros.sys]: VER: 1.1: Capabilities         0x000000000000f1ef
[mavros_node-1] [INFO] [1633956455.284105679] [mavros.sys]: VER: 1.1: Flight software:     04010000 (9bde2941)
[mavros_node-1] [INFO] [1633956455.284111420] [mavros.sys]: VER: 1.1: Middleware software: 00000000 (        )
[mavros_node-1] [INFO] [1633956455.284116084] [mavros.sys]: VER: 1.1: OS software:         00000000 (        )
[mavros_node-1] [INFO] [1633956455.284136938] [mavros.sys]: VER: 1.1: Board hardware:      00000000
[mavros_node-1] [INFO] [1633956455.284145564] [mavros.sys]: VER: 1.1: VID/PID:             0000:0000
[mavros_node-1] [INFO] [1633956455.284150424] [mavros.sys]: VER: 1.1: UID:                 0000000000000000
[main-2] [INFO] [1633956457.291223010] [mavrover.mav_rover]: vehicle is armed!
[mavros_node-1] [INFO] [1633956464.266090959] [mavros.home_position]: HP: requesting home position
[mavros_node-1] [INFO] [1633956464.287793524] [mavros.sys]: FCU: ArduRover V4.1.0-dev (9bde2941)
[mavros_node-1] [INFO] [1633956464.287911271] [mavros.sys]: FCU: 20485ecaeb6c4dd2906535330ee0840e
[mavros_node-1] [INFO] [1633956464.795831748] [mavros.param]: PR: parameters list received
[mavros_node-1] [INFO] [1633956469.286629679] [mavros.mission]: WP: mission received
[mavros_node-1] [INFO] [1633956474.310155687] [mavros.rallypoint]: RP: mission received
[mavros_node-1] [INFO] [1633956479.287770278] [mavros.geofence]: GF: mission received

Diagnostics

place here result of:
header:
  stamp:
    sec: 1633957537
    nanosec: 839229852
  frame_id: ''
status:
- level: "\0"
  name: 'mavros_router: MAVROS Router'
  message: ok
  hardware_id: none
  values:
  - key: Endpoints
    value: '3'
  - key: Messages routed
    value: '106860'
  - key: Messages sent
    value: '213707'
  - key: Messages dropped
    value: '0'
- level: "\0"
  name: 'mavros_router: endpoint 1000: udp://:14550@'
  message: ok
  hardware_id: none
  values:
  - key: Received packets
    value: '41204'
  - key: Dropped packets
    value: '0'
  - key: Buffer overruns
    value: '0'
  - key: Parse errors
    value: '0'
  - key: Rx sequence number
    value: '146'
  - key: Tx sequence number
    value: '0'
  - key: Rx total bytes
    value: '3521279'
  - key: Tx total bytes
    value: '3099'
  - key: Rx speed
    value: '3189.000000'
  - key: Tx speed
    value: '0.000000'
  - key: Remotes count
    value: '3'
  - key: Remote [0]
    value: '0.0'
  - key: Remote [1]
    value: '1.0'
  - key: Remote [2]
    value: '1.1'
- level: "\0"
  name: 'mavros_router: endpoint 1001: tcp-l://:5670'
  message: ok
  hardware_id: none
  values:
  - key: Received packets
    value: '0'
  - key: Dropped packets
    value: '0'
  - key: Buffer overruns
    value: '0'
  - key: Parse errors
    value: '0'
  - key: Rx sequence number
    value: '0'
  - key: Tx sequence number
    value: '0'
  - key: Rx total bytes
    value: '0'
  - key: Tx total bytes
    value: '0'
  - key: Rx speed
    value: '0.000000'
  - key: Tx speed
    value: '0.000000'
  - key: Remotes count
    value: '1'
  - key: Remote [0]
    value: '0.0'
- level: "\0"
  name: 'mavros_router: endpoint 1002: /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: 1633957537
    nanosec: 844314080
  frame_id: ''
status:
- level: "\0"
  name: 'mavros: MAVROS UAS'
  message: connected
  hardware_id: uas:///uas1
  values: []
- level: "\0"
  name: 'mavros: GPS'
  message: 3D fix
  hardware_id: uas:///uas1
  values:
  - key: Satellites visible
    value: '10'
  - key: Fix type
    value: '6'
  - key: EPH (m)
    value: '1.21'
  - key: EPV (m)
    value: '2.00'
- level: "\0"
  name: 'mavros: System'
  message: Normal
  hardware_id: uas:///uas1
  values:
  - key: Sensor present
    value: '0x1320DC2F'
  - key: Sensor enabled
    value: '0x1320802F'
  - key: Sensor health
    value: '0x1330812F'
  - 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: Ok
  - key: Logging
    value: Ok
  - key: Battery
    value: Ok
  - key: pre-arm check status. Always healthy when armed
    value: Ok
  - key: CPU Load (%)
    value: '0.0'
  - 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: "\0"
  name: 'mavros: Battery'
  message: Normal
  hardware_id: uas:///uas1
  values:
  - key: Voltage
    value: '12.59'
  - key: Current
    value: '0.0'
  - key: Remaining
    value: '100.0'
- level: "\0"
  name: 'mavros: Heartbeat'
  message: Normal
  hardware_id: uas:///uas1
  values:
  - key: Heartbeats since startup
    value: '1084'
  - key: Frequency (Hz)
    value: '0.999963'
  - key: Vehicle type
    value: Ground rover
  - key: Autopilot type
    value: ArduPilot
  - key: Mode
    value: MANUAL
  - key: System status
    value: ACTIVE

Check ID

Router topic: /uas1/mavlink_source, target: 1.1
ERROR. I got 0 addresses, but not your target 1.1
---
Received 0, from 0 addresses
address   list of messages
owais36 commented 3 years ago

This is also a problem I am facing. We are using some of the servo output raw channels and we use channels after ch 8. However, the mavlink v2.0 is unable to get any servo output raw message. We had those channels before but they stopped with pribably any new changes pushed on the repo. Can you tell me if that is the case and how to resolve it?

esharet commented 3 years ago

I don't know how to help you with that question because we are trying to control only ch1&ch3. Our mavlink version is v2

Rezenders commented 2 years ago

Override commands are only allowed from the GCS, so you have to change the mavros system and component id to match the expected id from the GCS, which is 255 and 240.


mavros_node:
  ros__parameters:
    fcu_url: udp://:14550@
    gcs_url: tcp-l://:5670
    tgt_system: 1
    tgt_component : 1
    system_id: 255
    component_id: 240
vooon commented 2 years ago

Try to see what's coming to override topic. But also please note that you're node will sit in minimal_mavrover = My_MavRover() forever and do not spin the node.

NotAlessandroBeatini commented 1 year ago

Override commands are only allowed from the GCS, so you have to change the mavros system and component id to match the expected id from the GCS, which is 255 and 240.


mavros_node:
  ros__parameters:
    fcu_url: udp://:14550@
    gcs_url: tcp-l://:5670
    tgt_system: 1
    tgt_component : 1
    system_id: 255
    component_id: 240

Hello, both ros2 and ardupilot noob here, how can i do that? to what config/launch file do i have to refer to? Thanks in advance for your help !