search

mavlink / mavros

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

mavros commands about controlling orientation , acceleration and angular speed didn't work #1412

Open unravelwool opened 4 years ago

unravelwool commented 4 years ago

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

Issue details

I'm using Mavros to communicate with Ardupilot SITL to control my UAV in Gazebo simulator. When I test the mavros methods of controlling uav by publishing some messages to mavros/setpoint_xxx/xxxx topic, I find some controlling comands do not work. Please see the following description for details. 1) Publish messages to mavros/setpoint_accel/accel topic, the uavs in both ardupilot SITL and Gazebo don't move, the command controlling acceleration doesn't work. 2) Publish messgaes to mavros/setpoint_attitude/cmd_vel topic, the uavs in both ardupilot SITL and Gazebo don't move. 3) Publish messgaes to mavros/setpoint_position/local topic, the command set the target position and orientation the uav get. However, the uav in simulation can only translate to the target position, but does not achieve the target orientation required by the command. 4) Publish messages to mavros/setpoint_velocity/cmd_vel topic, the messgaes set the value of x/y/z axis linear speed and roll/pitch/yaw angular speed, but the uav in simulation can only fly in translation method, and don't rotate. above mavros command sent to apm sitl is all in guided mode (a flight mode in apm) and after takeoff. Summary: the mavros can set the position and x/y/z axis speed of uav, but can not control the angles and angular speeds of uav.

Is something about my mavros wrong? or some param about mavros is set wrong? How can I control the yaw angle and angular speed? Besides, I am let mavros communicate to Ardupilot SITL using the apm_sitl.launch file like this:

<launch>
    <!-- vim: set ft=xml noet : -->
    <!-- example launch script for ArduPilotMega based FCU's  -->

    <arg name="fcu_url" default="udp://127.0.0.1:14550@" />
    <arg name="gcs_url" default="" />
    <arg name="tgt_system" default="1" />
    <arg name="tgt_component" default="1" />
    <arg name="log_output" default="screen" />

    <include file="$(find mavros)/launch/node.launch">
        <arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
        <arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
        <arg name="fcu_url" value="$(arg fcu_url)" />
        <arg name="gcs_url" value="$(arg gcs_url)" />
        <arg name="tgt_system" value="$(arg tgt_system)" />
        <arg name="tgt_component" value="$(arg tgt_component)" />
        <arg name="log_output" value="$(arg log_output)" />
    </include>
</launch>

MAVROS version and platform

Mavros: ?0.18.4? maybe, I have forget the version of mavros. how I can get the version information. ROS: Kinetic Ubuntu: 16.04

Autopilot type and version

[x] ArduPilot [ ] PX4

Version: Copter V3.6-dev (c6a0763b)

Node logs

SUMMARY
========

CLEAR PARAMETERS
 * /group1/mavros/

PARAMETERS
 * /group1/mavros/cmd/use_comp_id_system_control: False
 * /group1/mavros/conn/heartbeat_mav_type: ONBOARD_CONTROLLER
 * /group1/mavros/conn/heartbeat_rate: 1.0
 * /group1/mavros/conn/system_time_rate: 1.0
 * /group1/mavros/conn/timeout: 10.0
 * /group1/mavros/conn/timesync_rate: 0.0
 * /group1/mavros/distance_sensor/rangefinder_pub/field_of_view: 0.0
 * /group1/mavros/distance_sensor/rangefinder_pub/frame_id: lidar
 * /group1/mavros/distance_sensor/rangefinder_pub/id: 0
 * /group1/mavros/distance_sensor/rangefinder_pub/send_tf: False
 * /group1/mavros/distance_sensor/rangefinder_pub/sensor_position/x: 0.0
 * /group1/mavros/distance_sensor/rangefinder_pub/sensor_position/y: 0.0
 * /group1/mavros/distance_sensor/rangefinder_pub/sensor_position/z: -0.1
 * /group1/mavros/distance_sensor/rangefinder_sub/id: 1
 * /group1/mavros/distance_sensor/rangefinder_sub/orientation: PITCH_270
 * /group1/mavros/distance_sensor/rangefinder_sub/subscriber: True
 * /group1/mavros/fake_gps/eph: 2.0
 * /group1/mavros/fake_gps/epv: 2.0
 * /group1/mavros/fake_gps/fix_type: 3
 * /group1/mavros/fake_gps/geo_origin/alt: 408.0
 * /group1/mavros/fake_gps/geo_origin/lat: 47.3667
 * /group1/mavros/fake_gps/geo_origin/lon: 8.55
 * /group1/mavros/fake_gps/gps_rate: 5.0
 * /group1/mavros/fake_gps/mocap_transform: True
 * /group1/mavros/fake_gps/satellites_visible: 5
 * /group1/mavros/fake_gps/tf/child_frame_id: fix
 * /group1/mavros/fake_gps/tf/frame_id: map
 * /group1/mavros/fake_gps/tf/listen: False
 * /group1/mavros/fake_gps/tf/rate_limit: 10.0
 * /group1/mavros/fake_gps/tf/send: False
 * /group1/mavros/fake_gps/use_mocap: True
 * /group1/mavros/fake_gps/use_vision: False
 * /group1/mavros/fcu_protocol: v2.0
 * /group1/mavros/fcu_url: udp://127.0.0.1:1...
 * /group1/mavros/gcs_url: 
 * /group1/mavros/global_position/child_frame_id: base_link
 * /group1/mavros/global_position/frame_id: map
 * /group1/mavros/global_position/gps_uere: 1.0
 * /group1/mavros/global_position/rot_covariance: 99999.0
 * /group1/mavros/global_position/tf/child_frame_id: base_link
 * /group1/mavros/global_position/tf/frame_id: map
 * /group1/mavros/global_position/tf/global_frame_id: earth
 * /group1/mavros/global_position/tf/send: False
 * /group1/mavros/global_position/use_relative_alt: True
 * /group1/mavros/image/frame_id: px4flow
 * /group1/mavros/imu/angular_velocity_stdev: 0.0003490659 // 0...
 * /group1/mavros/imu/frame_id: base_link
 * /group1/mavros/imu/linear_acceleration_stdev: 0.0003
 * /group1/mavros/imu/magnetic_stdev: 0.0
 * /group1/mavros/imu/orientation_stdev: 1.0
 * /group1/mavros/landing_target/camera/fov_x: 2.0071286398
 * /group1/mavros/landing_target/camera/fov_y: 2.0071286398
 * /group1/mavros/landing_target/image/height: 480
 * /group1/mavros/landing_target/image/width: 640
 * /group1/mavros/landing_target/land_target_type: VISION_FIDUCIAL
 * /group1/mavros/landing_target/listen_lt: False
 * /group1/mavros/landing_target/mav_frame: LOCAL_NED
 * /group1/mavros/landing_target/target_size/x: 0.3
 * /group1/mavros/landing_target/target_size/y: 0.3
 * /group1/mavros/landing_target/tf/child_frame_id: camera_center
 * /group1/mavros/landing_target/tf/frame_id: landing_target
 * /group1/mavros/landing_target/tf/listen: False
 * /group1/mavros/landing_target/tf/rate_limit: 10.0
 * /group1/mavros/landing_target/tf/send: True
 * /group1/mavros/local_position/frame_id: map
 * /group1/mavros/local_position/tf/child_frame_id: base_link
 * /group1/mavros/local_position/tf/frame_id: map
 * /group1/mavros/local_position/tf/send: False
 * /group1/mavros/local_position/tf/send_fcu: False
 * /group1/mavros/mission/pull_after_gcs: True
 * /group1/mavros/mocap/use_pose: True
 * /group1/mavros/mocap/use_tf: False
 * /group1/mavros/odometry/estimator_type: 3
 * /group1/mavros/odometry/frame_tf/desired_frame: ned
 * /group1/mavros/plugin_blacklist: ['actuator_contro...
 * /group1/mavros/plugin_whitelist: []
 * /group1/mavros/px4flow/frame_id: px4flow
 * /group1/mavros/px4flow/ranger_fov: 0.118682
 * /group1/mavros/px4flow/ranger_max_range: 5.0
 * /group1/mavros/px4flow/ranger_min_range: 0.3
 * /group1/mavros/safety_area/p1/x: 1.0
 * /group1/mavros/safety_area/p1/y: 1.0
 * /group1/mavros/safety_area/p1/z: 1.0
 * /group1/mavros/safety_area/p2/x: -1.0
 * /group1/mavros/safety_area/p2/y: -1.0
 * /group1/mavros/safety_area/p2/z: -1.0
 * /group1/mavros/setpoint_accel/send_force: False
 * /group1/mavros/setpoint_attitude/reverse_thrust: False
 * /group1/mavros/setpoint_attitude/tf/child_frame_id: target_attitude
 * /group1/mavros/setpoint_attitude/tf/frame_id: map
 * /group1/mavros/setpoint_attitude/tf/listen: False
 * /group1/mavros/setpoint_attitude/tf/rate_limit: 50.0
 * /group1/mavros/setpoint_attitude/use_quaternion: False
 * /group1/mavros/setpoint_position/mav_frame: LOCAL_NED
 * /group1/mavros/setpoint_position/tf/child_frame_id: target_position
 * /group1/mavros/setpoint_position/tf/frame_id: map
 * /group1/mavros/setpoint_position/tf/listen: False
 * /group1/mavros/setpoint_position/tf/rate_limit: 50.0
 * /group1/mavros/setpoint_raw/thrust_scaling: 1.0
 * /group1/mavros/setpoint_velocity/mav_frame: LOCAL_NED
 * /group1/mavros/startup_px4_usb_quirk: False
 * /group1/mavros/sys/disable_diag: False
 * /group1/mavros/sys/min_voltage: 10.0
 * /group1/mavros/target_component_id: 1
 * /group1/mavros/target_system_id: 1
 * /group1/mavros/tdr_radio/low_rssi: 40
 * /group1/mavros/time/time_ref_source: fcu
 * /group1/mavros/time/timesync_avg_alpha: 0.6
 * /group1/mavros/time/timesync_mode: MAVLINK
 * /group1/mavros/vibration/frame_id: base_link
 * /group1/mavros/vision_pose/tf/child_frame_id: vision_estimate
 * /group1/mavros/vision_pose/tf/frame_id: map
 * /group1/mavros/vision_pose/tf/listen: False
 * /group1/mavros/vision_pose/tf/rate_limit: 10.0
 * /group1/mavros/vision_speed/listen_twist: True
 * /group1/mavros/vision_speed/twist_cov: True
 * /group1/mavros/wheel_odometry/child_frame_id: base_link
 * /group1/mavros/wheel_odometry/count: 2
 * /group1/mavros/wheel_odometry/frame_id: map
 * /group1/mavros/wheel_odometry/send_raw: True
 * /group1/mavros/wheel_odometry/send_twist: False
 * /group1/mavros/wheel_odometry/tf/child_frame_id: base_link
 * /group1/mavros/wheel_odometry/tf/frame_id: map
 * /group1/mavros/wheel_odometry/tf/send: True
 * /group1/mavros/wheel_odometry/use_rpm: False
 * /group1/mavros/wheel_odometry/vel_error: 0.1
 * /group1/mavros/wheel_odometry/wheel0/radius: 0.05
 * /group1/mavros/wheel_odometry/wheel0/x: 0.0
 * /group1/mavros/wheel_odometry/wheel0/y: -0.15
 * /group1/mavros/wheel_odometry/wheel1/radius: 0.05
 * /group1/mavros/wheel_odometry/wheel1/x: 0.0
 * /group1/mavros/wheel_odometry/wheel1/y: 0.15
 * /group1/robot_description: <?xml version="1....
 * /group1/tf_prefix: tf_prefix_1
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
 * /use_sim_time: True

NODES
  /group1/
    joint_state_publisher (joint_state_publisher/joint_state_publisher)
    mavros (mavros/mavros_node)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    spawn_erlecopter (gazebo_ros/spawn_model)
  /
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)

ROS_MASTER_URI=http://localhost:11311

process[group1/mavros-1]: started with pid [13649]
process[group1/spawn_erlecopter-2]: started with pid [13650]
process[group1/robot_state_publisher-3]: started with pid [13651]
process[group1/joint_state_publisher-4]: started with pid [13652]
process[gazebo-5]: started with pid [13653]
process[gazebo_gui-6]: started with pid [13657]
[ INFO] [1587378737.031457145]: FCU URL: udp://127.0.0.1:14550@
[ INFO] [1587378737.032666231]: udp0: Bind address: 127.0.0.1:14550
[ INFO] [1587378737.032732851]: GCS bridge disabled
[ INFO] [1587378737.096798779]: Plugin 3dr_radio loaded
[ INFO] [1587378737.098536839]: Plugin 3dr_radio initialized
[ INFO] [1587378737.098562912]: Plugin actuator_control blacklisted
[ INFO] [1587378737.098571832]: Plugin altitude blacklisted
[ INFO] [1587378737.098630052]: Plugin command loaded
[ INFO] [1587378737.104179422]: Plugin command initialized
[ INFO] [1587378737.104209541]: Plugin ftp blacklisted
[ INFO] [1587378737.104291118]: Plugin global_position loaded
[ INFO] [1587378737.133957470]: Plugin global_position initialized
[ INFO] [1587378737.133986573]: Plugin hil blacklisted
[ INFO] [1587378737.134052388]: Plugin home_position loaded
[ INFO] [1587378737.137579642]: Plugin home_position initialized
[ INFO] [1587378737.137694061]: Plugin imu loaded
[ INFO] [1587378737.148498972]: Plugin imu initialized
[ INFO] [1587378737.148721348]: Plugin local_position loaded
[ INFO] [1587378737.172356216]: Plugin local_position initialized
[ INFO] [1587378737.172432943]: Plugin manual_control loaded
[ INFO] [1587378737.175426965]: Plugin manual_control initialized
[ INFO] [1587378737.175529615]: Plugin param loaded
[ INFO] [1587378737.177863453]: Plugin param initialized
[ INFO] [1587378737.177970898]: Plugin rc_io loaded
[ INFO] [1587378737.184989327]: Plugin rc_io initialized
[ INFO] [1587378737.185052491]: Plugin safety_area blacklisted
[ INFO] [1587378737.185157447]: Plugin setpoint_accel loaded
[ INFO] [1587378737.189205466]: Plugin setpoint_accel initialized
[ INFO] [1587378737.189309308]: Plugin setpoint_attitude loaded
[ INFO] [1587378737.202899783]: Plugin setpoint_attitude initialized
[ INFO] [1587378737.202998023]: Plugin setpoint_position loaded
SpawnModel script started
[ INFO] [1587378737.233510587]: Plugin setpoint_position initialized
[ INFO] [1587378737.233591702]: Plugin setpoint_raw loaded
[ INFO] [1587378737.241467416]: Plugin setpoint_raw initialized
[ INFO] [1587378737.241550339]: Plugin setpoint_trajectory loaded
[ INFO] [1587378737.245446293]: Plugin setpoint_trajectory initialized
[ INFO] [1587378737.245516273]: Plugin setpoint_velocity loaded
[ INFO] [1587378737.249888335]: Plugin setpoint_velocity initialized
[ INFO] [1587378737.249970469]: Plugin sys_status loaded
[ INFO] [1587378737.260772571]: Plugin sys_status initialized
[ INFO] [1587378737.260847901]: Plugin sys_time loaded
[ INFO] [1587378737.276906449]: TM: Timesync mode: MAVLINK
[ INFO] [1587378737.278201050]: Plugin sys_time initialized
[ INFO] [1587378737.278310295]: Plugin vfr_hud loaded
[ INFO] [1587378737.279330910]: Plugin vfr_hud initialized
[ INFO] [1587378737.279460204]: Plugin waypoint loaded
[ INFO] [1587378737.284470323]: Plugin waypoint initialized
[ INFO] [1587378737.284605483]: Plugin wind_estimation loaded
[ INFO] [1587378737.285252341]: Plugin wind_estimation initialized
[ INFO] [1587378737.285298640]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1587378737.285313370]: Built-in MAVLink package version: 2019.12.30
[ INFO] [1587378737.285334712]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad icarous matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1587378737.285348086]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[INFO] [1587378737.346374, 0.000000]: Loading model XML from ros parameter
[INFO] [1587378737.350307, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
Gazebo multi-robot simulator, version 7.0.0
Copyright (C) 2012-2016 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

Diagnostics

rostopic echo -n1 /diagnostics
header: 
  seq: 527
  stamp: 
    secs: 539
    nsecs:         0
  frame_id: ''
status: 
  - 
    level: 0
    name: "group1/mavros: FCU connection"
    message: "connected"
    hardware_id: "udp://127.0.0.1:14550@"
    values: 
      - 
        key: "Received packets:"
        value: "64138"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "138"
      - 
        key: "Tx sequence number:"
        value: "0"
      - 
        key: "Rx total bytes:"
        value: "1967987"
      - 
        key: "Tx total bytes:"
        value: "110989"
      - 
        key: "Rx speed:"
        value: "3453.000000"
      - 
        key: "Tx speed:"
        value: "687.000000"
  - 
    level: 0
    name: "group1/mavros: GPS"
    message: "3D fix"
    hardware_id: "udp://127.0.0.1:14550@"
    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: "group1/mavros: Heartbeat"
    message: "Normal"
    hardware_id: "udp://127.0.0.1:14550@"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "535"
      - 
        key: "Frequency (Hz)"
        value: "1.047619"
      - 
        key: "Vehicle type"
        value: "Quadrotor"
      - 
        key: "Autopilot type"
        value: "ArduPilot"
      - 
        key: "Mode"
        value: "GUIDED"
      - 
        key: "System status"
        value: "Active"
  - 
    level: 0
    name: "group1/mavros: System"
    message: "Normal"
    hardware_id: "udp://127.0.0.1:14550@"
    values: 
      - 
        key: "Sensor present"
        value: "0x0361FDAF"
      - 
        key: "Sensor enabled"
        value: "0x0361FDAF"
      - 
        key: "Sensor health"
        value: "0x0361FDAF"
      - 
        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: "computer vision position"
        value: "Ok"
      - 
        key: "laser based position"
        value: "Ok"
      - 
        key: "3D angular rate control"
        value: "Ok"
      - 
        key: "attitude stabilization"
        value: "Ok"
      - 
        key: "yaw position"
        value: "Ok"
      - 
        key: "z/altitude control"
        value: "Ok"
      - 
        key: "x/y position control"
        value: "Ok"
      - 
        key: "motor outputs / control"
        value: "Ok"
      - 
        key: "rc receiver"
        value: "Ok"
      - 
        key: "AHRS subsystem health"
        value: "Ok"
      - 
        key: "Terrain subsystem health"
        value: "Ok"
      - 
        key: "Logging"
        value: "Ok"
      - 
        key: "Battery"
        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: "group1/mavros: Battery"
    message: "Normal"
    hardware_id: "udp://127.0.0.1:14550@"
    values: 
      - 
        key: "Voltage"
        value: "12.19"
      - 
        key: "Current"
        value: "28.7"
      - 
        key: "Remaining"
        value: "61.0"
---

Check ID

rosrun mavros checkid
ERROR. I got 0 addresses, but not your target 1:1

---
Received 0 messages, from 0 addresses
sys:comp   list of messages
PotatoesLegend commented 4 years ago

Hi @unravelwool , Did you progress on this task? I'm also trying to control a UAV from sitl with mavros and gazebo simulation.

mariofaragalla commented 3 years ago

I had similar issues and here is my conclusion:

1- For the topic /mavros/setpoint_position/global :I checked the type of messages published on it by running rostopic info /mavros/setpoint_position/global I found out that the message type that I should be working with is geographic_msgs/GeoPoseStamped not mavros_msgs/GlobalPositionTarget (I don't know why the documentation is not updated) and this worked for me.

2- For the topic mavros/setpoint_attitude/cmd_vel : it didn't work for the velocity but the topic mavros/setpoint_velocity/cmd_vel (which is not mentioned in the documentation but can be found in the setpoint.py file which can be found in /catkin_ws/src/mavros/mavros/src/mavros ) worked for me.

3- For the topic mavros/setpoint_accel/accel : unfortunately I am still trying to figure out how to make it work (maybe that's the reason)