Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Question: ros2 topic info /mavros/setpoint_velocity/cmd_vel(_unstamped) - Ardupilot Mode? #1870

Closed
Scoeerg opened this issue Jun 29, 2023 · 9 comments

Comments

@Scoeerg
Copy link

Scoeerg commented Jun 29, 2023

Issue details

I would like to control the robot in MAVROS2 using geometry_msgs/msg/Twist, since it is the "standard" interface to control a robot in ROS2, see e.g.

  • ROS2 Teleop Keyboard
  • Nav2 Interface Method is called when a new velocity command is demanded by the controller server in-order for the robot to follow the global path. This method returns a geometry_msgs::msg::TwistStamped which represents the velocity command for the robot to drive.

Expected behavior:

After running the Mavros node (Details, see below) and then publishing Twist-Messages into different topics:

ros2 topic pub /mavros/setpoint_velocity/cmd_vel geometry_msgs/msg/TwistStamped '{linear: {x: 1.0}}'

which answers with

publisher: beginning loop
publishing #1: geometry_msgs.msg.TwistStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), twist=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=1.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)))
ros2 topic pub /mavros/setpoint_velocity/cmd_vel geometry_msgs/msg/TwistStamped '{ header: {frame_id:  "/base_link"}, twist: { linear: { x: 0.2, y: 0 }, angular: { x: 0, y: 0, z: 2} } }'

which answers with

publisher: beginning loop
publishing #1: geometry_msgs.msg.TwistStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id='/base_link'), twist=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.2, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=2.0)))
ros2 topic pub /mavros/setpoint_velocity/cmd_vel_unstamped geometry_msgs/msg/TwistStamped '{twist: {linear: {x: 1.0}}}'

which answers with:

publishing #1: geometry_msgs.msg.TwistStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), twist=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=1.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)))

The robot should start moving. Question:

What mode should be Ardupilot be in? GUIDED? MANUAL? What about the frame_id?

MAVROS version and platform

Mavros: 2.5.0, installed from source
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] [1688031417.856952353] [mavros_node]: Starting mavros router node
[INFO] [1688031417.859287560] [mavros_router]: Built-in SIMD instructions: SSE, SSE2
[INFO] [1688031417.859305124] [mavros_router]: Built-in MAVLink package version: 2022.12.30
[INFO] [1688031417.859314535] [mavros_router]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all cubepilot development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[INFO] [1688031417.859322816] [mavros_router]: MAVROS Router started
[INFO] [1688031417.859393533] [mavros_router]: Requested to add endpoint: type: 0, url: /dev/ttyUSB0:57600
[INFO] [1688031417.859549512] [mavros_router]: Endpoint link[1000] created
[INFO] [1688031417.860859192] [mavros_router]: link[1000] opened successfully
[INFO] [1688031417.860963717] [mavros_router]: Requested to add endpoint: type: 2, url: /uas1
[INFO] [1688031417.861098377] [mavros_router]: Endpoint link[1001] created
[INFO] [1688031417.861960267] [mavros_router]: link[1001] opened successfully
[INFO] [1688031417.861996166] [mavros_node]: Starting mavros uas node
[INFO] [1688031417.867988140] [mavros_router]: link[1000] detected remote address 1.1
[INFO] [1688031417.886922003] [mavros]: UAS Executor started, threads: 16
[INFO] [1688031417.897677182] [mavros]: Plugin actuator_control created
[INFO] [1688031417.897905342] [mavros]: Plugin actuator_control initialized
[INFO] [1688031417.910074456] [mavros]: Plugin adsb created
[INFO] [1688031417.910262345] [mavros]: Plugin adsb initialized
[INFO] [1688031417.912516883] [mavros]: Plugin altitude created
[INFO] [1688031417.912628456] [mavros]: Plugin altitude initialized
[INFO] [1688031417.914778640] [mavros]: Plugin cam_imu_sync created
[INFO] [1688031417.914878596] [mavros]: Plugin cam_imu_sync initialized
[INFO] [1688031417.917276433] [mavros]: Plugin camera created
[INFO] [1688031417.917432319] [mavros]: Plugin camera initialized
[INFO] [1688031417.920563305] [mavros]: Plugin cellular_status created
[INFO] [1688031417.920605549] [mavros]: Plugin cellular_status initialized
[INFO] [1688031417.925696014] [mavros]: Plugin command created
[INFO] [1688031417.925828599] [mavros]: Plugin command initialized
[INFO] [1688031417.928347344] [mavros]: Plugin companion_process_status created
[INFO] [1688031417.928371368] [mavros]: Plugin companion_process_status initialized
[INFO] [1688031417.931661668] [mavros]: Plugin debug_value created
[INFO] [1688031417.931914815] [mavros]: Plugin debug_value initialized
[INFO] [1688031417.939464611] [mavros.distance_sensor]: DS: Plugin not configured!
[INFO] [1688031417.939562049] [mavros]: Plugin distance_sensor created
[INFO] [1688031417.939674224] [mavros]: Plugin distance_sensor initialized
[INFO] [1688031417.947875676] [mavros]: Plugin esc_status created
[INFO] [1688031417.948075454] [mavros]: Plugin esc_status initialized
[INFO] [1688031417.951062184] [mavros]: Plugin esc_telemetry created
[INFO] [1688031417.951331073] [mavros]: Plugin esc_telemetry initialized
[INFO] [1688031417.961804662] [mavros]: Plugin fake_gps created
[INFO] [1688031417.961845508] [mavros]: Plugin fake_gps initialized
[INFO] [1688031417.980288163] [mavros]: Plugin ftp created
[INFO] [1688031417.980442932] [mavros]: Plugin ftp initialized
[INFO] [1688031417.990803263] [mavros]: Plugin geofence created
[INFO] [1688031417.991110228] [mavros]: Plugin geofence initialized
[INFO] [1688031418.005604644] [mavros]: Plugin global_position created
[INFO] [1688031418.005856651] [mavros]: Plugin global_position initialized
[INFO] [1688031418.020223720] [mavros]: Plugin gps_input created
[INFO] [1688031418.020264615] [mavros]: Plugin gps_input initialized
[INFO] [1688031418.030515128] [mavros]: Plugin gps_rtk created
[INFO] [1688031418.030642685] [mavros]: Plugin gps_rtk initialized
[INFO] [1688031418.041185169] [mavros]: Plugin gps_status created
[INFO] [1688031418.041531706] [mavros]: Plugin gps_status initialized
[INFO] [1688031418.056796860] [mavros]: Plugin guided_target created
[INFO] [1688031418.056921830] [mavros]: Plugin guided_target initialized
[INFO] [1688031418.071801640] [mavros]: Plugin hil created
[INFO] [1688031418.071964852] [mavros]: Plugin hil initialized
[INFO] [1688031418.088005494] [mavros]: Plugin home_position created
[INFO] [1688031418.088127051] [mavros]: Plugin home_position initialized
[INFO] [1688031418.105370159] [mavros]: Plugin imu created
[INFO] [1688031418.105698420] [mavros]: Plugin imu initialized
[INFO] [1688031418.123520647] [mavros]: Plugin landing_target created
[INFO] [1688031418.123645310] [mavros]: Plugin landing_target initialized
[INFO] [1688031418.138948241] [mavros_router]: link[1000] detected remote address 255.190
[INFO] [1688031418.146315317] [mavros]: Plugin local_position created
[INFO] [1688031418.146474194] [mavros]: Plugin local_position initialized
[INFO] [1688031418.164540661] [mavros]: Plugin log_transfer created
[INFO] [1688031418.164721110] [mavros]: Plugin log_transfer initialized
[INFO] [1688031418.181654999] [mavros]: Plugin mag_calibration_status created
[INFO] [1688031418.181848252] [mavros]: Plugin mag_calibration_status initialized
[INFO] [1688031418.199282596] [mavros]: Plugin manual_control created
[INFO] [1688031418.199415161] [mavros]: Plugin manual_control initialized
[INFO] [1688031418.216802162] [mavros]: Plugin mocap_pose_estimate created
[INFO] [1688031418.216841956] [mavros]: Plugin mocap_pose_estimate initialized
[INFO] [1688031418.236228900] [mavros]: Plugin mount_control created
[INFO] [1688031418.236400243] [mavros]: Plugin mount_control initialized
[INFO] [1688031418.253709003] [mavros]: Plugin nav_controller_output created
[INFO] [1688031418.253844521] [mavros]: Plugin nav_controller_output initialized
[INFO] [1688031418.271614869] [mavros]: Plugin obstacle_distance created
[INFO] [1688031418.271649214] [mavros]: Plugin obstacle_distance initialized
[INFO] [1688031418.290109433] [mavros]: Plugin odometry created
[INFO] [1688031418.290241075] [mavros]: Plugin odometry initialized
[INFO] [1688031418.308740619] [mavros]: Plugin onboard_computer_status created
[INFO] [1688031418.308777539] [mavros]: Plugin onboard_computer_status initialized
[INFO] [1688031418.327522845] [mavros]: Plugin param created
[INFO] [1688031418.327651783] [mavros]: Plugin param initialized
[INFO] [1688031418.346546982] [mavros]: Plugin play_tune created
[INFO] [1688031418.346583193] [mavros]: Plugin play_tune initialized
[INFO] [1688031418.372631392] [mavros]: Plugin px4flow created
[INFO] [1688031418.372789143] [mavros]: Plugin px4flow initialized
[INFO] [1688031418.392906288] [mavros]: Plugin rallypoint created
[INFO] [1688031418.393040567] [mavros]: Plugin rallypoint initialized
[INFO] [1688031418.411643688] [mavros]: Plugin rangefinder created
[INFO] [1688031418.411796223] [mavros]: Plugin rangefinder initialized
[INFO] [1688031418.438564109] [mavros]: Plugin rc_io created
[INFO] [1688031418.438761517] [mavros]: Plugin rc_io initialized
[INFO] [1688031418.458867201] [mavros]: Plugin setpoint_accel created
[INFO] [1688031418.458907874] [mavros]: Plugin setpoint_accel initialized
[INFO] [1688031418.484688428] [mavros]: Plugin setpoint_attitude created
[INFO] [1688031418.484731089] [mavros]: Plugin setpoint_attitude initialized
[INFO] [1688031418.512774800] [mavros]: Plugin setpoint_position created
[INFO] [1688031418.512820918] [mavros]: Plugin setpoint_position initialized
[INFO] [1688031418.541557123] [mavros]: Plugin setpoint_raw created
[INFO] [1688031418.541784937] [mavros]: Plugin setpoint_raw initialized
[INFO] [1688031418.569575842] [mavros]: Plugin setpoint_trajectory created
[INFO] [1688031418.569614687] [mavros]: Plugin setpoint_trajectory initialized
[INFO] [1688031418.597421357] [mavros]: Plugin setpoint_velocity created
[INFO] [1688031418.597458386] [mavros]: Plugin setpoint_velocity initialized
[INFO] [1688031418.638716123] [mavros]: Plugin sys_status created
[INFO] [1688031418.639133154] [mavros]: Plugin sys_status initialized
[INFO] [1688031418.660202804] [mavros.time]: TM: Timesync mode: MAVLINK
[INFO] [1688031418.663061191] [mavros]: Plugin sys_time created
[INFO] [1688031418.663224181] [mavros]: Plugin sys_time initialized
[INFO] [1688031418.691201042] [mavros]: Plugin tdr_radio created
[INFO] [1688031418.691375033] [mavros]: Plugin tdr_radio initialized
[INFO] [1688031418.712476325] [mavros]: Plugin terrain created
[INFO] [1688031418.712613774] [mavros]: Plugin terrain initialized
[INFO] [1688031418.742063174] [mavros]: Plugin trajectory created
[INFO] [1688031418.742190525] [mavros]: Plugin trajectory initialized
[INFO] [1688031418.771147085] [mavros]: Plugin tunnel created
[INFO] [1688031418.771274199] [mavros]: Plugin tunnel initialized
[INFO] [1688031418.798975340] [mavros]: Plugin vfr_hud created
[INFO] [1688031418.799134620] [mavros]: Plugin vfr_hud initialized
[INFO] [1688031418.821186490] [mavros]: Plugin vibration created
[INFO] [1688031418.821318269] [mavros]: Plugin vibration initialized
[INFO] [1688031418.850362239] [mavros]: Plugin vision_pose created
[INFO] [1688031418.850394921] [mavros]: Plugin vision_pose initialized
[INFO] [1688031418.884871300] [mavros]: Plugin vision_speed created
[INFO] [1688031418.884908099] [mavros]: Plugin vision_speed initialized
[INFO] [1688031418.922065624] [mavros]: Plugin waypoint created
[INFO] [1688031418.922305529] [mavros]: Plugin waypoint initialized
[INFO] [1688031418.956137385] [mavros]: Plugin wheel_odomotry created
[INFO] [1688031418.956308202] [mavros]: Plugin wheel_odomotry initialized
[INFO] [1688031418.981150259] [mavros]: Plugin wind_estimation created
[INFO] [1688031418.981336243] [mavros]: Plugin wind_estimation initialized
[INFO] [1688031418.990086959] [mavros]: Built-in SIMD instructions: SSE, SSE2
[INFO] [1688031418.990119447] [mavros]: Built-in MAVLink package version: 2022.12.30
[INFO] [1688031418.990129520] [mavros]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all cubepilot development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[INFO] [1688031418.990138178] [mavros]: MAVROS UAS via /uas1 started. MY ID 1.191, TARGET ID 1.1
[INFO] [1688031419.151992773] [mavros.rc]: RC_CHANNELS message detected!
[INFO] [1688031419.158970293] [mavros.imu]: IMU: Raw IMU message used.
[WARN] [1688031419.159272597] [mavros.imu]: IMU: linear acceleration on RAW_IMU known on APM only.
[WARN] [1688031419.159342539] [mavros.imu]: IMU: ~imu/data_raw stores unscaled raw acceleration report.
[INFO] [1688031419.220475511] [mavros]: CON: Got HEARTBEAT, connected. FCU: ArduPilot
[INFO] [1688031419.221187993] [mavros.mission]: WP: detected enable_partial_push: 1
[INFO] [1688031419.332925707] [mavros.rc]: RC_CHANNELS message detected!
[INFO] [1688031419.339864238] [mavros.imu]: IMU: Raw IMU message used.
[INFO] [1688031419.621454205] [mavros_router]: link[1001] detected remote address 1.191
[INFO] [1688031419.849517623] [mavros_router]: link[1000] detected remote address 51.68
[WARN] [1688031420.453439016] [mavros.cmd]: CMD: Unexpected command 520, result 0
[INFO] [1688031420.467775438] [mavros.geofence]: GF: Using MISSION_ITEM_INT
[INFO] [1688031420.467923027] [mavros.rallypoint]: RP: Using MISSION_ITEM_INT
[INFO] [1688031420.467990702] [mavros.mission]: WP: Using MISSION_ITEM_INT
[INFO] [1688031420.468086302] [mavros.sys]: VER: 1.1: Capabilities         0x000000000000f1ef
[INFO] [1688031420.468180622] [mavros.sys]: VER: 1.1: Flight software:     040203ff (2172cfb3)
[INFO] [1688031420.468251074] [mavros.sys]: VER: 1.1: Middleware software: 00000000 (        )
[INFO] [1688031420.468300452] [mavros.sys]: VER: 1.1: OS software:         00000000 (38022f4f   @WV)
[INFO] [1688031420.468341180] [mavros.sys]: VER: 1.1: Board hardware:      00320000
[INFO] [1688031420.468389473] [mavros.sys]: VER: 1.1: VID/PID:             1209:5740
[INFO] [1688031420.468445220] [mavros.sys]: VER: 1.1: UID:                 0000000000000000
[INFO] [1688031429.220835929] [mavros.home_position]: HP: requesting home position
[INFO] [1688031429.448024037] [mavros.sys]: FCU: ArduRover V4.2.3 (2172cfb3)
[INFO] [1688031429.452858486] [mavros.sys]: FCU: ChibiOS: 38022f4f
[INFO] [1688031429.460873995] [mavros.sys]: FCU: Pixhawk4 0049002D 34385105 36363539
[INFO] [1688031429.465860159] [mavros.sys]: FCU: RCOut: PWM:1-16
[INFO] [1688031429.474882310] [mavros.sys]: FCU: IMU0: fast sampling enabled 8.0kHz/2.0kHz

Diagnostics

ros2 topic echo /diagnostics:

header:
  stamp:
    sec: 1688031516
    nanosec: 858759969
  frame_id: ''
status:
- level: "\0"
  name: 'mavros_router: MAVROS Router'
  message: ok
  hardware_id: none
  values:
  - key: Endpoints
    value: '2'
  - key: Messages routed
    value: '8396'
  - key: Messages sent
    value: '8396'
  - 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: '8272'
  - key: Dropped packets
    value: '0'
  - key: Buffer overruns
    value: '0'
  - key: Parse errors
    value: '0'
  - key: Rx sequence number
    value: '108'
  - key: Tx sequence number
    value: '0'
  - key: Rx total bytes
    value: '266087'
  - key: Tx total bytes
    value: '2601'
  - key: Rx speed
    value: inf
  - key: Tx speed
    value: inf
  - key: Remotes count
    value: '7'
  - 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'
  - key: Remote [5]
    value: '255.0'
  - key: Remote [6]
    value: '255.190'
- 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: 1688031516
    nanosec: 863804561
  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: '6'
  - key: Fix type
    value: '3'
  - key: EPH (m)
    value: '3.31'
  - key: EPV (m)
    value: '3.36'
- level: "\x01"
  name: 'mavros: Mount'
  message: Can not diagnose in this targeting mode
  hardware_id: uas:///uas1
  values:
  - key: Mode
    value: '255'
- 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: '13.3'
  - 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: '98'
  - key: Frequency (Hz)
    value: '1.000030'
  - key: Vehicle type
    value: Ground rover
  - key: Autopilot type
    value: ArduPilot
  - key: Mode
    value: MANUAL
  - key: System status
    value: ACTIVE
- level: "\0"
  name: 'mavros: 3DR Radio'
  message: Normal
  hardware_id: uas:///uas1
  values:
  - key: RSSI
    value: '181'
  - key: RSSI (dBm)
    value: '-31.7'
  - key: Remote RSSI
    value: '184'
  - key: Remote RSSI (dBm)
    value: '-30.2'
  - key: Tx buffer (%)
    value: '100'
  - key: Noice level
    value: '60'
  - key: Remote noice level
    value: '70'
  - key: Rx errors
    value: '0'
  - key: Fixed
    value: '0'
---

and Sink ros2 topic echo /uas1/mavlink_source:

header:
  stamp:
    sec: 1688031542
    nanosec: 651477491
  frame_id: ep:1000
framing_status: 1
magic: 253
len: 9
incompat_flags: 0
compat_flags: 0
seq: 123
sysid: 51
compid: 68
msgid: 109
checksum: 45180
payload64:
- 4640035504115417088
- 4363133204008415050
signature: []

ROS2 Services and Topics

ros2 topic list:

/diagnostics
/mavros/actuator_control
/mavros/adsb/send
/mavros/adsb/vehicle
/mavros/altitude
/mavros/battery
/mavros/cam_imu_sync/cam_imu_stamp
/mavros/camera/image_captured
/mavros/cellular_status/status
/mavros/companion_process/status
/mavros/debug_value/debug
/mavros/debug_value/debug_float_array
/mavros/debug_value/debug_vector
/mavros/debug_value/named_value_float
/mavros/debug_value/named_value_int
/mavros/debug_value/send
/mavros/esc_status/info
/mavros/esc_status/status
/mavros/esc_telemetry/telemetry
/mavros/estimator_status
/mavros/extended_state
/mavros/fake_gps/mocap/tf
/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/gps_input/gps_input
/mavros/gps_rtk/rtk_baseline
/mavros/gps_rtk/send_rtcm
/mavros/gpsstatus/gps1/raw
/mavros/gpsstatus/gps1/rtk
/mavros/gpsstatus/gps2/raw
/mavros/gpsstatus/gps2/rtk
/mavros/hil/actuator_controls
/mavros/hil/controls
/mavros/hil/gps
/mavros/hil/imu_ned
/mavros/hil/optical_flow
/mavros/hil/rc_inputs
/mavros/hil/state
/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/landing_target/lt_marker
/mavros/landing_target/pose
/mavros/landing_target/pose_in
/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/log_transfer/raw/log_data
/mavros/log_transfer/raw/log_entry
/mavros/mag_calibration/report
/mavros/mag_calibration/status
/mavros/manual_control/control
/mavros/manual_control/send
/mavros/mission/reached
/mavros/mission/waypoints
/mavros/mocap/pose
/mavros/mocap/tf
/mavros/mount_control/command
/mavros/mount_control/orientation
/mavros/mount_control/status
/mavros/nav_controller_output/output
/mavros/obstacle/send
/mavros/odometry/in
/mavros/odometry/out
/mavros/onboard_computer/status
/mavros/param/event
/mavros/play_tune
/mavros/px4flow/ground_distance
/mavros/px4flow/raw/optical_flow_rad
/mavros/px4flow/raw/send
/mavros/px4flow/temperature
/mavros/radio_status
/mavros/rallypoint/rallypoints
/mavros/rangefinder/rangefinder
/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/terrain/report
/mavros/time_reference
/mavros/timesync_status
/mavros/trajectory/desired
/mavros/trajectory/generated
/mavros/trajectory/path
/mavros/tunnel/in
/mavros/tunnel/out
/mavros/vfr_hud
/mavros/vibration/raw/vibration
/mavros/vision_pose/pose
/mavros/vision_pose/pose_cov
/mavros/vision_speed/speed_twist
/mavros/vision_speed/speed_twist_cov
/mavros/vision_speed/speed_vector
/mavros/wheel_odometry/odom
/mavros/wind_estimation
/move_base_simple/goal
/parameter_events
/rosout
/tf
/tf_static
/uas1/mavlink_sink
/uas1/mavlink_source

ros2 service list:

/mavros/actuator_control/describe_parameters
...
/mavros_router/set_parameters_atomically
@vooon
Copy link
Member

vooon commented Jun 29, 2023

Hi,

Since all setpoint_foo uses SET_*_TARGET_* i guess that APM need GUIDED mode. PX4 - OFFBOARD.

@Scoeerg
Copy link
Author

Scoeerg commented Jun 30, 2023

Hey @vooon Thanks for the feedback. That is what I expected. Now when calling

ros2 topic echo /mavros/state

I will get info:

header:
  stamp:
    sec: 1688126251
    nanosec: 855269776
  frame_id: ''
connected: true
armed: true
guided: true
manual_input: false
mode: AUTO
system_status: 4

which tells me I am in guided mode hence guided: true. I accomplished this with the RC-controller. Some APM parameter defines the FlightMode to change to auto if I switch a specific switch on the RC. Now unfortunately, when trying to set flightmode in MAVROS using

ros2 service call /mavros/set_mode mavros_msgs/SetMode '{base_mode: 220}'

which according to the message documentation on mavros_msgs/SetMode should be MAV_MODE_AUTO_ARMED, the mavros_node responds with:

[WARN] [1688126325.408404494] [mavros.cmd]: CMD: Unexpected command 11, result 2

and when sending

ros2 service call /mavros/set_mode mavros_msgs/SetMode '{base_mode: 220, custom_mode: AUTO}'

it responds with

[WARN] [1688126574.210961789] [mavros.cmd]: CMD: Unexpected command 11, result 0

Any suggestions?

@vooon
Copy link
Member

vooon commented Jun 30, 2023

No, you literally should enable GUIDED mode, not guided-bit flag of base mode. I.e. custom_mode: "GUIDED".

@Scoeerg
Copy link
Author

Scoeerg commented Jul 3, 2023

Additional information on Body Frames can be found here (ROS1): #792

Ok. So far:

ros2 topic echo /mavros/state tells me I am armed and in GUIDED mode now:

header:
  stamp:
    sec: 1688385613
    nanosec: 112080595
  frame_id: ''
connected: true
armed: true
guided: true
manual_input: false
mode: GUIDED
system_status: 4

To send some twist message, I again wrote some minimal example publisher, see:

from geometry_msgs.msg import TwistStamped
from std_msgs.msg import Header
import rclpy
from rclpy.node import Node


class velocityPublisher(Node):


    def __init__(self):
        super().__init__('velocitypublisher')
        self.publisher_ = self.create_publisher(TwistStamped, '/mavros/setpoint_velocity/cmd_vel', 10)
        timer_period = 0.1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.frame_id = 'base_link'

    def timer_callback(self):
        msg = TwistStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = self.frame_id
        msg.twist.linear.x = 2.0
        msg.twist.linear.y = 2.0
        msg.twist.linear.z = 0.0
        msg.twist.angular.x = 0.0
        msg.twist.angular.y = 0.0
        msg.twist.angular.z = 0.0
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing: "%s"' %msg)

def main(args=None):
    rclpy.init(args=args)
    publisher = velocityPublisher()
    rclpy.spin(publisher)
    publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

and I will guess the frame_id is base_link, which is the local coordinate frame the robot is in, since ros2 topic echo /mavros/local_position/velocity_local answers with

header:
  stamp:
    sec: 1688385567
    nanosec: 740169483
    frame_id: base_link
twist:
  linear:
    x: 0.5530000329017639
    y: -0.07100000232458115
    z: 0.4710000157356262
  angular:
    x: -0.0010925584554715949
    y: 0.00018972645995732828
    z: 0.0010494333728826098

@vooon
Copy link
Member

vooon commented Jul 3, 2023

If i remember correctly plugin doesn't check frame_id. And i'm really unsure if Twist plugin has right type_mask for APM as it was initially developed for PX4.
I think better to do some experimentation with setpoint_raw.

@Scoeerg
Copy link
Author

Scoeerg commented Jul 4, 2023

Additional information can be found here:

  1. type_mask discussion: expose local_position_setpoint bitfield #402
  2. Message Definition mavros/PositionTarget
  3. Mavlink documentation on SET_POSITION_TARGET_LOCAL_NED

Based on your suggestion I worked on mavros/setpoint_raw/local which is a subscriber with PositionTarget msg. i.e. it takes

std_msgs/Header header
uint8 coordinate_frame
uint16 type_mask
geometry_msgs/Point position
geometry_msgs/Vector3 velocity
geometry_msgs/Vector3 acceleration_or_force
float32 yaw
float32 yaw_rate

messages. With the uint16 type_mask ignore Bitmask, I will ignore all except for velocity in x and y:

0 0 0 0 1 1 1 1 1 1 0 0 1 1 1 1
N/A N/A N/A N/A YawRate Yaw Force AFZ AFY AFX VZ VY VX PZ PY PX

which I believe will be binary 0000111111001111 and decimal 4047. I again wrote a minimal example publisher:

from mavros_msgs.msg import PositionTarget
from geometry_msgs.msg import Point
from geometry_msgs.msg import Vector3
import rclpy
from rclpy.node import Node

class positiontargetpublisher(Node):

    def __init__(self):
        super().__init__('positiontargetpublisher')
        self.publisher_ = self.create_publisher(PositionTarget, '/mavros/setpoint_raw/local', 10)
        timer_period = 0.1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.frame_id = 'FRAME_LOCAL_OFFSET_NED'

    def timer_callback(self):
        msg = PositionTarget()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = self.frame_id
        msg.coordinate_frame = 7
        msg.type_mask = 4047
        msg.position.x = 0.0
        msg.position.y = 0.0
        msg.position.z = 0.0
        msg.velocity.x = 10.0
        msg.velocity.y = 10.0
        msg.velocity.z = 0.0
        msg.acceleration_or_force.x = 0.0
        msg.acceleration_or_force.y = 0.0
        msg.acceleration_or_force.z = 0.0
        msg.yaw = 0.0
        msg.yaw_rate = 0.0
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing: "%s"' %msg)


def main(args=None):
    rclpy.init(args=args)
    publisher = positiontargetpublisher()
    rclpy.spin(publisher)
    publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

while in GUIDED mode, hence ros2 topic echo /mavros/state responds with:

header:
  stamp:
    sec: 1688462546
    nanosec: 723682750
  frame_id: ''
connected: true
armed: true
guided: true
manual_input: false
mode: GUIDED
system_status: 4

which does not work. :(

Weird little details that might help:

When publishing those messages in MANUAL flight mode (currently my default) and disarmed, nothing happens (as expected), BUT when switching to GUIDED armed the Rover seems to move following the commands previously sent.

@vooon
Copy link
Member

vooon commented Jul 4, 2023

Hello,

As i remember, you should start publishing setpoints and then switch to GUIDED.
Also even type_mask feeling like you may freely select only needed fields, actual implementation on FC side usually expects only few exact combinations. Usually best way to know them - dive into FCU code...

@Scoeerg
Copy link
Author

Scoeerg commented Jul 4, 2023

Hi,

this is all fun and games, but this:

As i remember, you should start publishing setpoints and then switch to GUIDED

means you cannot establish a velocity "live-stream" the rover (drone) follows as a reference, unless I keep toggling mode. i.e.

sampleTime = 50ms
while(True):
  set MANUAL mode
  writeVelocity(getCurrentVelocityIWant())
  set GUIDED mode 
  # robot actually drives with myvelocity
  wait(sampleTime)

What I've read about, too is FOLLOWING mode which enables a robot to follow the position of another robot (with an offset). This kind of sounds like one would be able to just "live-stream" a virtual's robot position/velocity. But I did not find this mode supported in MAVROS.

Also even type_mask feeling like you may freely select only needed fields, actual implementation on FC side usually expects only few exact combinations. Usually best way to know them - dive into FCU code...

Well this is just disappointing, but thank you for the heads-up.

@Scoeerg
Copy link
Author

Scoeerg commented Jul 13, 2023

Hi,

as stated earlier, GUIDED mode is not made for reference tracking for the Rover/Drone.

In Control Theory/Robotics terms read this robotics.stackexchange question on difference path/trajectory planning:

  • GUIDED mode for handing a planned trajectory to the drone/rover
  • AUTO mode for handing over a planned path to the drone/rover

but none of them are made for my intention: reference tracking. The closest implementation to reference tracking seems to be the FOLLOWING mode. But it seems it only follows position, rather than the entire state space of the system (position & velocity).

My solution:

The standard way to give commands to a robot in ROS is using a Twist Message, as e.g. Teleop Twist Keyboard in the early turtlebot tutorials or in NAV2.

To now follow the twist message as a reference, I implemented a Node that subscribes any Twist message (topic \twist in the code) and publishes a Mavros OverrideRCIn (topic \mavros\rc\override in code). Read about RC-Override in my earlier issue: #1866

Be aware this code is rather specific to your situation, as it depends on parameters set in Ardupilot and your robot. Those are:

self.steeringMin = 1100 
self.steeringMax = 1900
self.throttleMin = 1100
self.throttleMax = 1900
self.angularVelocityMax = 0.5
self.angularVelocityMin = (-1)*self.angularVelocityMax
self.velocityMax = 2.78 #m/s
self.velocityMin = (-1)*self.velocityMax

msg.channels = [STEERING, 0, THROTTLE, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

mapping(twistMessage)
    ...
    return steering, throttle 

The node "translating" Twist to RCOverride:

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

class Twist2RcOverride(Node):

    def __init__(self):
        super().__init__('rcoverride_publisher')
        self.rcoverridepublisher = self.create_publisher(OverrideRCIn, '/mavros/rc/override', 10)
        self.twistsubscription = self.create_subscription(Twist, '/twist', self.twistcallback, 10)
        timer_period = 0.05  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        ### <---- Parameters ---- >
        self.steeringMin = 1100
        self.steeringMax = 1900
        self.throttleMin = 1100
        self.throttleMax = 1900
        self.angularVelocityMax = 0.5
        self.angularVelocityMin = (-1)*self.angularVelocityMax
        self.velocityMax = 2.78
        self.velocityMin = (-1)*self.velocityMax
        ### <---- Parameters ---- >
        # Initializing
        self.steering = 1500
        self.throttle = 1500

    def timer_callback(self):
        msg = OverrideRCIn()
        # msg.channels = [STEERING, 0, THROTTLE, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        msg.channels = [int(self.steering), 0, int(self.throttle), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        self.rcoverridepublisher.publish(msg)
        self.get_logger().debug('Published RC-Override to Pixhawk using MavROS: %s' %(" ".join([str(x) for x in msg.channels])))

    def twistcallback(self, twistMessage):
        self.get_logger().debug('I heard: %s' %(str(twistMessage)))
        self.steering, self.throttle = self.mapping(twistMessage)

    def mapping(self, twistMessage):
        steering = 1500 + 400*twistMessage.angular.z/self.angularVelocityMax
        throttle = 1500 + 400*twistMessage.linear.x/self.velocityMax
        if steering < self.steeringMin:
            steering = self.steeringMin
        elif steering > self.steeringMax:
            steering = self.steeringMax
        if throttle < self.throttleMin:
            throttle = self.throttleMin
        elif throttle > self.throttleMax:
            throttle = self.throttleMax
        return steering, throttle


def main(args=None):
    rclpy.init(args=args)
    node = Twist2RcOverride()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()
if __name__ == '__main__':
    main()

which works just fine, hence I will close the issue. But be aware: the solution is not ideal, my hope is that there is a closed control loop on the robot that somehow digests the RC-channels to references in velocity, otherwise it's open loop. To know, one would need to dig through the Ardupilot code. Thanks @vooon :)

@Scoeerg Scoeerg closed this as completed Jul 13, 2023
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants