mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
910 stars 997 forks source link

/mavros/rc/override Topic Not Working for PX4 #1900

Open mescar-nmt opened 1 year ago

mescar-nmt commented 1 year ago

Issue details

Hello, I am setting up a custom UAV with a Pixhawk 2.4.8 flight computer for an experiment. This experiment requires custom RC inputs. After investigating the best method of controlling the RC inputs, I decided to use the /mavros/rc/override topic. However, once I publish my command, nothing happens. The topic surely publishes on the /mavros/rc/override topic, but it does not translate to the /mavros/rc/in or /mavros/rc/out topics. Perhaps my setup is incorrect, or I have to change mavros source code to get the override topic to work... Any help is appreciated.

MAVROS version and platform

Mavros: 1.17.0 ROS: Noetic Ubuntu: 20.04

Autopilot type and version

[ ] ArduPilot [ x ] PX4

Version: 1.13.3

Node logs

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /mavros/camera/frame_id: base_link
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 1.0
 * /mavros/conn/system_time_rate: 1.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 10.0
 * /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
 * /mavros/distance_sensor/hrlv_ez4_pub/id: 0
 * /mavros/distance_sensor/hrlv_ez4_pub/orientation: PITCH_270
 * /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/laser_1_sub/id: 3
 * /mavros/distance_sensor/laser_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/laser_1_sub/subscriber: True
 * /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
 * /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
 * /mavros/distance_sensor/lidarlite_pub/id: 1
 * /mavros/distance_sensor/lidarlite_pub/orientation: PITCH_270
 * /mavros/distance_sensor/lidarlite_pub/send_tf: True
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/sonar_1_sub/horizontal_fov_ratio: 1.0
 * /mavros/distance_sensor/sonar_1_sub/id: 2
 * /mavros/distance_sensor/sonar_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/distance_sensor/sonar_1_sub/vertical_fov_ratio: 1.0
 * /mavros/fake_gps/eph: 2.0
 * /mavros/fake_gps/epv: 2.0
 * /mavros/fake_gps/fix_type: 3
 * /mavros/fake_gps/geo_origin/alt: 408.0
 * /mavros/fake_gps/geo_origin/lat: 47.3667
 * /mavros/fake_gps/geo_origin/lon: 8.55
 * /mavros/fake_gps/gps_rate: 5.0
 * /mavros/fake_gps/mocap_transform: True
 * /mavros/fake_gps/satellites_visible: 5
 * /mavros/fake_gps/tf/child_frame_id: fix
 * /mavros/fake_gps/tf/frame_id: map
 * /mavros/fake_gps/tf/listen: False
 * /mavros/fake_gps/tf/rate_limit: 10.0
 * /mavros/fake_gps/tf/send: False
 * /mavros/fake_gps/use_mocap: True
 * /mavros/fake_gps/use_vision: False
 * /mavros/fcu_protocol: v2.0
 * /mavros/fcu_url: /dev/ttyUSB0:57600
 * /mavros/gcs_url: 
 * /mavros/global_position/child_frame_id: base_link
 * /mavros/global_position/frame_id: map
 * /mavros/global_position/gps_uere: 1.0
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: base_link
 * /mavros/global_position/tf/frame_id: map
 * /mavros/global_position/tf/global_frame_id: earth
 * /mavros/global_position/tf/send: False
 * /mavros/global_position/use_relative_alt: True
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.0003490659 // 0...
 * /mavros/imu/frame_id: base_link
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/landing_target/camera/fov_x: 2.0071286398
 * /mavros/landing_target/camera/fov_y: 2.0071286398
 * /mavros/landing_target/image/height: 480
 * /mavros/landing_target/image/width: 640
 * /mavros/landing_target/land_target_type: VISION_FIDUCIAL
 * /mavros/landing_target/listen_lt: False
 * /mavros/landing_target/mav_frame: LOCAL_NED
 * /mavros/landing_target/target_size/x: 0.3
 * /mavros/landing_target/target_size/y: 0.3
 * /mavros/landing_target/tf/child_frame_id: camera_center
 * /mavros/landing_target/tf/frame_id: landing_target
 * /mavros/landing_target/tf/listen: False
 * /mavros/landing_target/tf/rate_limit: 10.0
 * /mavros/landing_target/tf/send: True
 * /mavros/local_position/frame_id: map
 * /mavros/local_position/tf/child_frame_id: base_link
 * /mavros/local_position/tf/frame_id: map
 * /mavros/local_position/tf/send: False
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mission/use_mission_item_int: True
 * /mavros/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/mount/debounce_s: 4.0
 * /mavros/mount/err_threshold_deg: 10.0
 * /mavros/mount/negate_measured_pitch: False
 * /mavros/mount/negate_measured_roll: False
 * /mavros/mount/negate_measured_yaw: False
 * /mavros/odometry/fcu/odom_child_id_des: base_link
 * /mavros/odometry/fcu/odom_parent_id_des: map
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682
 * /mavros/px4flow/ranger_max_range: 5.0
 * /mavros/px4flow/ranger_min_range: 0.3
 * /mavros/safety_area/p1/x: 1.0
 * /mavros/safety_area/p1/y: 1.0
 * /mavros/safety_area/p1/z: 1.0
 * /mavros/safety_area/p2/x: -1.0
 * /mavros/safety_area/p2/y: -1.0
 * /mavros/safety_area/p2/z: -1.0
 * /mavros/setpoint_accel/send_force: False
 * /mavros/setpoint_attitude/reverse_thrust: False
 * /mavros/setpoint_attitude/tf/child_frame_id: target_attitude
 * /mavros/setpoint_attitude/tf/frame_id: map
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 50.0
 * /mavros/setpoint_attitude/use_quaternion: False
 * /mavros/setpoint_position/mav_frame: LOCAL_NED
 * /mavros/setpoint_position/tf/child_frame_id: target_position
 * /mavros/setpoint_position/tf/frame_id: map
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /mavros/setpoint_raw/thrust_scaling: 1.0
 * /mavros/setpoint_velocity/mav_frame: LOCAL_NED
 * /mavros/startup_px4_usb_quirk: False
 * /mavros/sys/disable_diag: False
 * /mavros/sys/min_voltage: 10.0
 * /mavros/target_component_id: 1
 * /mavros/target_system_id: 1
 * /mavros/tdr_radio/low_rssi: 40
 * /mavros/time/time_ref_source: fcu
 * /mavros/time/timesync_avg_alpha: 0.6
 * /mavros/time/timesync_mode: MAVLINK
 * /mavros/vibration/frame_id: base_link
 * /mavros/vision_pose/tf/child_frame_id: vision_estimate
 * /mavros/vision_pose/tf/frame_id: odom
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: True
 * /mavros/vision_speed/twist_cov: True
 * /mavros/wheel_odometry/child_frame_id: base_link
 * /mavros/wheel_odometry/count: 2
 * /mavros/wheel_odometry/frame_id: odom
 * /mavros/wheel_odometry/send_raw: True
 * /mavros/wheel_odometry/send_twist: False
 * /mavros/wheel_odometry/tf/child_frame_id: base_link
 * /mavros/wheel_odometry/tf/frame_id: odom
 * /mavros/wheel_odometry/tf/send: False
 * /mavros/wheel_odometry/use_rpm: False
 * /mavros/wheel_odometry/vel_error: 0.1
 * /mavros/wheel_odometry/wheel0/radius: 0.05
 * /mavros/wheel_odometry/wheel0/x: 0.0
 * /mavros/wheel_odometry/wheel0/y: -0.15
 * /mavros/wheel_odometry/wheel1/radius: 0.05
 * /mavros/wheel_odometry/wheel1/x: 0.0
 * /mavros/wheel_odometry/wheel1/y: 0.15
 * /rosdistro: noetic
 * /rosversion: 1.16.0

NODES
  /
    mavros (mavros/mavros_node)

auto-starting new master
process[master]: started with pid [1803039]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 7774b936-616e-11ee-96d7-2ddfd0fc0e8a
process[rosout-1]: started with pid [1803067]
started core service [/rosout]
process[mavros-2]: started with pid [1803075]
[ INFO] [1696283758.397431850]: FCU URL: /dev/ttyUSB0:57600
[ INFO] [1696283758.398878302]: serial0: device: /dev/ttyUSB0 @ 57600 bps
[ INFO] [1696283758.513964561]: GCS bridge disabled
[ INFO] [1696283758.542228557]: Plugin 3dr_radio loaded
[ INFO] [1696283758.547613831]: Plugin 3dr_radio initialized
[ INFO] [1696283758.547794442]: Plugin actuator_control loaded
[ INFO] [1696283758.554881987]: Plugin actuator_control initialized
[ INFO] [1696283758.555180576]: Plugin altitude loaded
[ INFO] [1696283758.557503568]: Plugin altitude initialized
[ INFO] [1696283758.557663243]: Plugin command loaded
[ INFO] [1696283758.573917679]: Plugin command initialized
[ INFO] [1696283758.574097231]: Plugin ftp loaded
[ INFO] [1696283758.587752812]: Plugin ftp initialized
[ INFO] [1696283758.587888545]: Plugin geofence loaded
[ INFO] [1696283758.592224106]: Plugin geofence initialized
[ INFO] [1696283758.592325927]: Plugin global_position loaded
[ INFO] [1696283758.606838975]: Plugin global_position initialized
[ INFO] [1696283758.606889532]: Plugin hil loaded
[ INFO] [1696283758.613202878]: Plugin hil initialized
[ INFO] [1696283758.613243607]: Plugin home_position loaded
[ INFO] [1696283758.614970747]: Plugin home_position initialized
[ INFO] [1696283758.615006967]: Plugin imu loaded
[ INFO] [1696283758.618621641]: Plugin imu initialized
[ INFO] [1696283758.618727203]: Plugin local_position loaded
[ INFO] [1696283758.622106393]: Plugin local_position initialized
[ INFO] [1696283758.622151742]: Plugin manual_control loaded
[ INFO] [1696283758.623810432]: Plugin manual_control initialized
[ INFO] [1696283758.623855384]: Plugin nav_controller_output loaded
[ INFO] [1696283758.624199661]: Plugin nav_controller_output initialized
[ INFO] [1696283758.624243801]: Plugin param loaded
[ INFO] [1696283758.625793492]: Plugin param initialized
[ INFO] [1696283758.625840034]: Plugin rallypoint loaded
[ INFO] [1696283758.627667479]: Plugin rallypoint initialized
[ INFO] [1696283758.627712005]: Plugin rc_io loaded
[ INFO] [1696283758.629853952]: Plugin rc_io initialized
[ INFO] [1696283758.629874199]: Plugin safety_area blacklisted
[ INFO] [1696283758.629916877]: Plugin setpoint_accel loaded
[ INFO] [1696283758.631569974]: Plugin setpoint_accel initialized
[ INFO] [1696283758.631629201]: Plugin setpoint_attitude loaded
[ INFO] [1696283758.636394678]: Plugin setpoint_attitude initialized
[ INFO] [1696283758.636439162]: Plugin setpoint_position loaded
[ INFO] [1696283758.643850551]: Plugin setpoint_position initialized
[ INFO] [1696283758.643898198]: Plugin setpoint_raw loaded
[ INFO] [1696283758.648198291]: Plugin setpoint_raw initialized
[ INFO] [1696283758.648237847]: Plugin setpoint_trajectory loaded
[ INFO] [1696283758.650481602]: Plugin setpoint_trajectory initialized
[ INFO] [1696283758.650520739]: Plugin setpoint_velocity loaded
[ INFO] [1696283758.653207610]: Plugin setpoint_velocity initialized
[ INFO] [1696283758.653266620]: Plugin sys_status loaded
[ INFO] [1696283758.658207039]: Plugin sys_status initialized
[ INFO] [1696283758.658248614]: Plugin sys_time loaded
[ INFO] [1696283758.660876609]: TM: Timesync mode: MAVLINK
[ INFO] [1696283758.661082928]: TM: Not publishing sim time
[ INFO] [1696283758.661647557]: Plugin sys_time initialized
[ INFO] [1696283758.661685337]: Plugin vfr_hud loaded
[ INFO] [1696283758.661975940]: Plugin vfr_hud initialized
[ INFO] [1696283758.662010944]: Plugin waypoint loaded
[ INFO] [1696283758.664260432]: Plugin waypoint initialized
[ INFO] [1696283758.664300614]: Plugin wind_estimation loaded
[ INFO] [1696283758.664671427]: Plugin wind_estimation initialized
[ INFO] [1696283758.664725847]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1696283758.664742902]: Built-in MAVLink package version: 2023.9.9
[ INFO] [1696283758.664754531]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all csAirLink cubepilot development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[ INFO] [1696283758.664766903]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1696283758.851469122]: RC_CHANNELS message detected!
[ INFO] [1696283759.275157086]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1696283759.355816409]: RC_CHANNELS message detected!
[ INFO] [1696283760.519567461]: GF: Using MISSION_ITEM_INT
[ INFO] [1696283760.519686099]: RP: Using MISSION_ITEM_INT
[ INFO] [1696283760.519764371]: WP: Using MISSION_ITEM_INT
[ INFO] [1696283760.519854309]: VER: 1.1: Capabilities         0x000000000000e4ff
[ INFO] [1696283760.519937429]: VER: 1.1: Flight software:     010d03ff (1c8ab2a0d7000000)
[ INFO] [1696283760.520000208]: VER: 1.1: Middleware software: 010d03ff (1c8ab2a0d7000000)
[ INFO] [1696283760.520079943]: VER: 1.1: OS software:         0b0000ff (4a1dd8680cd29f51)
[ INFO] [1696283760.520137626]: VER: 1.1: Board hardware:      00000011
[ INFO] [1696283760.520197713]: VER: 1.1: VID/PID:             26ac:0011
[ INFO] [1696283760.520238656]: VER: 1.1: UID:                 524e500220373442

Diagnostics

header: 
  seq: 42
  stamp: 
    secs: 1696283872
    nsecs: 743208879
  frame_id: ''
status: 
  - 
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "/dev/ttyUSB0:57600"
    values: 
      - 
        key: "Received packets:"
        value: "1560"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "182"
      - 
        key: "Tx sequence number:"
        value: "0"
      - 
        key: "Rx total bytes:"
        value: "61241"
      - 
        key: "Tx total bytes:"
        value: "19288"
      - 
        key: "Rx speed:"
        value: "937.000000"
      - 
        key: "Tx speed:"
        value: "384.000000"
  - 
    level: 2
    name: "mavros: GPS"
    message: "No satellites"
    hardware_id: "/dev/ttyUSB0:57600"
    values: 
      - 
        key: "Satellites visible"
        value: "0"
      - 
        key: "Fix type"
        value: "0"
      - 
        key: "EPH (m)"
        value: "Unknown"
      - 
        key: "EPV (m)"
        value: "Unknown"
  - 
    level: 0
    name: "mavros: Heartbeat"
    message: "Normal"
    hardware_id: "/dev/ttyUSB0:57600"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "48"
      - 
        key: "Frequency (Hz)"
        value: "1.000001"
      - 
        key: "Vehicle type"
        value: "Fixed wing aircraft"
      - 
        key: "Autopilot type"
        value: "PX4 Autopilot"
      - 
        key: "Mode"
        value: "AUTO.LAND"
      - 
        key: "System status"
        value: "Critical"
  - 
    level: 2
    name: "mavros: System"
    message: "Sensor health"
    hardware_id: "/dev/ttyUSB0:57600"
    values: 
      - 
        key: "Sensor present"
        value: "0x9021802C"
      - 
        key: "Sensor enabled"
        value: "0x9021800C"
      - 
        key: "Sensor health"
        value: "0x9020800F"
      - 
        key: "3D magnetometer"
        value: "Ok"
      - 
        key: "absolute pressure"
        value: "Ok"
      - 
        key: "motor outputs / control"
        value: "Ok"
      - 
        key: "rc receiver"
        value: "Fail"
      - 
        key: "AHRS subsystem health"
        value: "Ok"
      - 
        key: "pre-arm check status. Always healthy when armed"
        value: "Ok"
      - 
        key: "CPU Load (%)"
        value: "35.9"
      - 
        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: "/dev/ttyUSB0:57600"
    values: 
      - 
        key: "Voltage"
        value: "65.54"
      - 
        key: "Current"
        value: "-0.0"
      - 
        key: "Remaining"
        value: "-1.0"
  - 
    level: 0
    name: "mavros: Time Sync"
    message: "Normal"
    hardware_id: "/dev/ttyUSB0:57600"
    values: 
      - 
        key: "Timesyncs since startup"
        value: "449"
      - 
        key: "Frequency (Hz)"
        value: "10.071442"
      - 
        key: "Last RTT (ms)"
        value: "124.737890"
      - 
        key: "Mean RTT (ms)"
        value: "248.875189"
      - 
        key: "Last remote time (s)"
        value: "7836.285960000"
      - 
        key: "Estimated time offset (s)"
        value: "0.000000000"

Check ID

Received 445 messages, from 1 addresses
sys:comp   list of messages
  1:1     0, 65, 1, 36, 4, 230, 8, 74, 141, 77, 111, 83, 245, 22, 281, 411, 285, 30
lei-zihao commented 6 months ago

你好,请问这是什么原因呢,我又有遇到这种情况。