search

mavlink / mavros

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

Plugin setpoint_raw/attitude does not publish if I set the bitmask to IGNORE_ATTITUDE #675

Closed MAVREEF closed 7 years ago

MAVREEF commented 7 years ago

Issue details

I am controlling a quad using the setpoint_raw/attitude plugin. I would like to send rate commands instead of quaternions but when I set the bitmask to IGNORE_ATTITUDE the loopback topic "/mavros/setpoint_raw/target_attitude" stops publishing. Here is how I'm coding it:

mavros_msgs::AttitudeTarget att_setp;

att_setp.type_mask = mavros_msgs::AttitudeTarget::IGNORE_ATTITUDE;
.
.
.
//orientation and rate data for pixhawk commands
        att_setp.orientation.x = manual_output_att.x;
        att_setp.orientation.y = manual_output_att.y;
        att_setp.orientation.z = manual_output_att.z;
        att_setp.orientation.w = manual_output_att.w;
        att_setp.body_rate.x = .1;
        att_setp.body_rate.y = .1;
        att_setp.body_rate.z = .1;

// use for OFFBOARD autonomous control
        att_setp.thrust = auto_thrust_out;

//publishes control message
        att_setp.header.stamp = `rosTimeCurrent;`
        att_pub.publish(att_setp);

Any help is very much appreciated!

Respectfully,

MAVREEF

MAVROS version and platform

Mavros: ?0.17.5? ROS: ?Indigo? Ubuntu: ?14.04?

Autopilot type and version

[ ] ArduPilot [ X ] PX4

Version: ?1.5.5?

Node logs

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /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: ROLL_180
 * /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: ROLL_180
 * /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: ROLL_180
 * /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/id: 2
 * /mavros/distance_sensor/sonar_1_sub/orientation: ROLL_180
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/fcu_url: /dev/ttyUSB0:921600
 * /mavros/gcs_url: udp://@192.168.1.107
 * /mavros/global_position/frame_id: fcu
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: fcu_utm
 * /mavros/global_position/tf/frame_id: local_origin
 * /mavros/global_position/tf/send: False
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.000349065850399
 * /mavros/imu/frame_id: fcu
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/local_position/frame_id: fcu
 * /mavros/local_position/tf/child_frame_id: fcu
 * /mavros/local_position/tf/frame_id: local_origin
 * /mavros/local_position/tf/send: False
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mocap/use_pose: False
 * /mavros/mocap/use_tf: True
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.0
 * /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_throttle: False
 * /mavros/setpoint_attitude/tf/child_frame_id: attitude
 * /mavros/setpoint_attitude/tf/frame_id: local_origin
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 10.0
 * /mavros/setpoint_position/tf/child_frame_id: setpoint
 * /mavros/setpoint_position/tf/frame_id: local_origin
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /mavros/startup_px4_usb_quirk: True
 * /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/vibration/frame_id: vibration
 * /mavros/vision_pose/tf/child_frame_id: vision
 * /mavros/vision_pose/tf/frame_id: local_origin
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: False
 * /rosdistro: indigo
 * /rosversion: 1.11.20
 * /use_sim_time: False
 * /z_controller/throttle_offset: 0.645
 * /z_controller/z_e_over: -2
 * /z_controller/z_e_under: 0.3
 * /z_controller/z_kD: 0.1
 * /z_controller/z_kI: 0.01
 * /z_controller/z_kP: 0.1
 * /z_controller/z_max: 0.67
 * /z_controller/z_min: 0.57
 * /z_controller/z_spt: 1.0

NODES
  /
    goose (ros_vrpn_client/ros_vrpn_client)
    mavros (mavros/mavros_node)
    ros_estimator (ros_estimator/ros_estimator_node)
    z_controller (qz_controller/qz_controller_node)

Diagnostics

place here result of:
rostopic echo -n1 /diagnostics

header: 
  seq: 35
  stamp: 
    secs: 1489184298
    nsecs:  32863682
  frame_id: ''
status: 
  - 
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: /dev/ttyUSB0:921600
    values: 
      - 
        key: Received packets:
        value: 21796
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 145
      - 
        key: Tx sequence number:
        value: 64
      - 
        key: Rx total bytes:
        value: 859974
      - 
        key: Tx total bytes:
        value: 115346
      - 
        key: Rx speed:
        value: 20367.000000
      - 
        key: Tx speed:
        value: 2884.000000
  - 
    level: 0
    name: mavros: GCS bridge
    message: connected
    hardware_id: /dev/ttyUSB0:921600
    values: 
      - 
        key: Received packets:
        value: 40
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 114
      - 
        key: Tx sequence number:
        value: 0
      - 
        key: Rx total bytes:
        value: 680
      - 
        key: Tx total bytes:
        value: 858942
      - 
        key: Rx speed:
        value: 17.000000
      - 
        key: Tx speed:
        value: 20380.000000
  - 
    level: 0
    name: mavros: Heartbeat
    message: Normal
    hardware_id: /dev/ttyUSB0:921600
    values: 
      - 
        key: Heartbeats since startup
        value: 40
      - 
        key: Frequency (Hz)
        value: 0.999965
      - 
        key: Vehicle type
        value: Quadrotor
      - 
        key: Autopilot type
        value: PX4
      - 
        key: Mode
        value: OFFBOARD
      - 
        key: System status
        value: Standby
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/ttyUSB0:921600
    values: 
      - 
        key: Sensor present
        value: 0x00020000
      - 
        key: Sensor enabled
        value: 0x00020000
      - 
        key: Sensor helth
        value: 0x00020000
      - 
        key: Sensor 3D Gyro 2
        value: Ok
      - 
        key: CPU Load (%)
        value: 65.5
      - 
        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:921600
    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:921600
    values: 
      - 
        key: Timesyncs since startup
        value: 399
      - 
        key: Frequency (Hz)
        value: 10.070999
      - 
        key: Last dt (ms)
        value: -1.427935
      - 
        key: Mean dt (ms)
        value: 0.001462
      - 
        key: Last system time (s)
        value: 8325.988326000
      - 
        key: Time offset (s)
        value: 1489175972.040629864

Check ID

rosrun mavros checkid

OK. I got messages from 1:1.

---
Received 7710 messages, from 1 addresses
sys:comp   list of messages
  1:1     0, 1, 2, 132, 140, 141, 147, 30, 31, 32, 36, 65, 70, 74, 76, 87, 230, 231, 105, 111, 241, 245
vooon commented 7 years ago

I don't see any problems on mavros side. Look at receiver: https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp#L1175 And loopback sender: https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_messages.cpp#L2671

And here your problem on line 2675: if (_att_sp_sub->update(&_att_sp_time, &att_sp)) {

TSC21 commented 7 years ago

@MAVREEF updates?

TSC21 commented 7 years ago

Closing this as there are no updates. Reopen if needed.

fabrizioschiano commented 4 years ago

@MAVREEF any news on this? I know it is a long time ago but I would be interested to know if you advanced on your issue.