search

mavlink / mavros

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

Mode stuck on LOITER instead of switching to GUIDED and will not run #968

Closed onyezem96 closed 6 years ago

onyezem96 commented 6 years ago

Issue details

My code cannot change the mode of my UAV from LOITER to GUIDED for some reason. Because of this my code will not run properly and arm the motors. What can I do to force GUIDED to show up and have my motors properly arm?

MAVROS version and platform

Mavros: ?0.18.4? ROS: Kinetic Ubuntu: 16.04

Autopilot type and version

[ X ] ArduPilot [ ] PX4

Version: ?3.7.1?

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: 0.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 0.0
 * /mavros/distance_sensor/rangefinder_pub/field_of_view: 0.0
 * /mavros/distance_sensor/rangefinder_pub/frame_id: lidar
 * /mavros/distance_sensor/rangefinder_pub/id: 0
 * /mavros/distance_sensor/rangefinder_pub/send_tf: False
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/rangefinder_sub/id: 1
 * /mavros/distance_sensor/rangefinder_sub/orientation: PITCH_270
 * /mavros/distance_sensor/rangefinder_sub/subscriber: True
 * /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/ttyACM0:115200
 * /mavros/gcs_url: udp://:14560@127....
 * /mavros/global_position/child_frame_id: base_link
 * /mavros/global_position/frame_id: map
 * /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.000349065850399
 * /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/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/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/odometry/estimator_type: 3
 * /mavros/odometry/frame_tf/desired_frame: ned
 * /mavros/plugin_blacklist: ['actuator_contro...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682389136
 * /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_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: NONE
 * /mavros/vibration/frame_id: base_link
 * /mavros/vision_pose/tf/child_frame_id: vision_estimate
 * /mavros/vision_pose/tf/frame_id: map
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: False
 * /rosdistro: kinetic
 * /rosversion: 1.12.7

NODES
  /
    mavros (mavros/mavros_node)
    offb_node (hector_path_follower/offb_node)

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

setting /run_id to ee799036-219d-11e8-9e3a-d0577b40b3a2
process[rosout-1]: started with pid [24149]
started core service [/rosout]
process[mavros-2]: started with pid [24167]
process[offb_node-3]: started with pid [24168]
[ INFO] [1520382283.558664836]: Offboard node started
[ INFO] [1520382283.623243871]: FCU URL: /dev/ttyACM0:115200
[ INFO] [1520382283.625246627]: serial0: device: /dev/ttyACM0 @ 115200 bps
[ INFO] [1520382283.625874382]: GCS URL: udp://:14560@127.0.0.1:14550
[ INFO] [1520382283.626187335]: udp1: Bind address: 0.0.0.0:14560
[ INFO] [1520382283.626245420]: udp1: Remote address: 127.0.0.1:14550
[ INFO] [1520382283.659456206]: Plugin 3dr_radio loaded
[ INFO] [1520382283.661650347]: Plugin 3dr_radio initialized
[ INFO] [1520382283.661677317]: Plugin actuator_control blacklisted
[ INFO] [1520382283.687157156]: Plugin adsb loaded
[ INFO] [1520382283.691273977]: Plugin adsb initialized
[ INFO] [1520382283.691302950]: Plugin altitude blacklisted
[ INFO] [1520382283.691423509]: Plugin cam_imu_sync loaded
[ INFO] [1520382283.692152071]: Plugin cam_imu_sync initialized
[ INFO] [1520382283.692328875]: Plugin command loaded
[ INFO] [1520382283.697479667]: Plugin command initialized
[ INFO] [1520382283.697508663]: Plugin debug_value blacklisted
[ INFO] [1520382283.697679269]: Plugin distance_sensor loaded
[ INFO] [1520382283.706766859]: Plugin distance_sensor initialized
[ INFO] [1520382283.706965789]: Plugin fake_gps loaded
[ INFO] [1520382283.721139690]: Plugin fake_gps initialized
[ INFO] [1520382283.721170296]: Plugin ftp blacklisted
[ INFO] [1520382283.721323169]: Plugin global_position loaded
[ INFO] [1520382283.736520193]: Plugin global_position initialized
[ INFO] [1520382283.736600321]: Plugin hil blacklisted
[ INFO] [1520382283.736758822]: Plugin home_position loaded
[ INFO] [1520382283.741340970]: Plugin home_position initialized
[ INFO] [1520382283.741527242]: Plugin imu loaded
[ INFO] [1520382283.749926818]: Plugin imu initialized
[ INFO] [1520382283.750087801]: Plugin local_position loaded
[ INFO] [1520382283.755853315]: Plugin local_position initialized
[ INFO] [1520382283.756022257]: Plugin manual_control loaded
[ INFO] [1520382283.759795549]: Plugin manual_control initialized
[ INFO] [1520382283.759830228]: Plugin mocap_pose_estimate blacklisted
[ INFO] [1520382283.759960335]: Plugin obstacle_distance loaded
[ INFO] [1520382283.762664053]: Plugin obstacle_distance initialized
[ INFO] [1520382283.762798382]: Plugin odom loaded
[ INFO] [1520382283.767517459]: Plugin odom initialized
[ INFO] [1520382283.767773338]: Plugin param loaded
[ INFO] [1520382283.770770880]: Plugin param initialized
[ INFO] [1520382283.770834615]: Plugin px4flow blacklisted
[ INFO] [1520382283.770975738]: Plugin rangefinder loaded
[ INFO] [1520382283.771712229]: Plugin rangefinder initialized
[ INFO] [1520382283.771878086]: Plugin rc_io loaded
[ INFO] [1520382283.776139627]: Plugin rc_io initialized
[ INFO] [1520382283.776172970]: Plugin safety_area blacklisted
[ INFO] [1520382283.776325015]: Plugin setpoint_accel loaded
[ INFO] [1520382283.780292610]: Plugin setpoint_accel initialized
[ INFO] [1520382283.780562683]: Plugin setpoint_attitude loaded
[ INFO] [1520382283.791852304]: Plugin setpoint_attitude initialized
[ INFO] [1520382283.792124437]: Plugin setpoint_position loaded
[ INFO] [1520382283.805369742]: Plugin setpoint_position initialized
[ INFO] [1520382283.805534236]: Plugin setpoint_raw loaded
[ INFO] [1520382283.816455792]: Plugin setpoint_raw initialized
[ INFO] [1520382283.816623379]: Plugin setpoint_velocity loaded
[ INFO] [1520382283.823453941]: Plugin setpoint_velocity initialized
[ INFO] [1520382283.823800514]: Plugin sys_status loaded
[ INFO] [1520382283.830660838]: Plugin sys_status initialized
[ INFO] [1520382283.830858759]: Plugin sys_time loaded
[ INFO] [1520382283.833942031]: TM: Timesync mode: NONE
[ INFO] [1520382283.834545137]: Plugin sys_time initialized
[ INFO] [1520382283.834713725]: Plugin vfr_hud loaded
[ INFO] [1520382283.835944517]: Plugin vfr_hud initialized
[ INFO] [1520382283.835976743]: Plugin vibration blacklisted
[ INFO] [1520382283.835998205]: Plugin vision_pose_estimate blacklisted
[ INFO] [1520382283.836016779]: Plugin vision_speed_estimate blacklisted
[ INFO] [1520382283.836208822]: Plugin waypoint loaded
[ INFO] [1520382283.842180356]: Plugin waypoint initialized
[ INFO] [1520382283.842298085]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1520382283.842323129]: Built-in MAVLink package version: 2018.2.2
[ INFO] [1520382283.842345297]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1520382283.842362118]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1520382284.454682324]: udp1: Remote address: 127.0.0.1:14550
[ INFO] [1520382285.869628505]: CON: Got HEARTBEAT, connected. FCU: ArduPilotMega / ArduCopter
[ERROR] [1520382285.894104049]: FCU: Calibrating barometer
[ERROR] [1520382287.441284387]: FCU: barometer calibration complete
[ERROR] [1520382287.445146836]: FCU: GROUND START
[ WARN] [1520382287.875123497]: VER: broadcast request timeout, retries left 4
[ INFO] [1520382288.633611446]: IMU: Raw IMU message used.
[ WARN] [1520382288.645767243]: GP: No GPS fix
[ WARN] [1520382288.682788139]: TM: Wrong FCU time.
[ WARN] [1520382288.875093881]: VER: broadcast request timeout, retries left 3
[ WARN] [1520382294.875548046]: VER: unicast request timeout, retries left 2
[ INFO] [1520382295.870315292]: HP: requesting home position
GUIDED_NOGPS
LOITER
[ WARN] [1520382299.879659505]: VER: unicast request timeout, retries left 1
[ WARN] [1520382299.880787245]: PR: request list timeout, retries left 2
[ WARN] [1520382300.881709317]: PR: request list timeout, retries left 1
STATE: LOITER
[ INFO] [1520382301.168318822]: GUIDED enabled
[ WARN] [1520382301.875723362]: WP: timeout, retries left 2
[ WARN] [1520382301.882101594]: PR: request list timeout, retries left 0
[ WARN] [1520382302.877084943]: WP: timeout, retries left 1
[ WARN] [1520382303.878326152]: WP: timeout, retries left 0
[ERROR] [1520382304.878715349]: WP: timed out.
[ WARN] [1520382304.886707378]: VER: unicast request timeout, retries left 0
[ INFO] [1520382305.871302233]: HP: requesting home position
[ WARN] [1520382309.891569892]: VER: your FCU don't support AUTOPILOT_VERSION, switched to default capabilities
STATE: LOITER
[ INFO] [1520382309.893192236]: GUIDED enabled
STATE: LOITER
[ INFO] [1520382314.897590110]: GUIDED enabled
[ INFO] [1520382315.870184437]: HP: requesting home position
[ WARN] [1520382318.871495457]: GP: No GPS fix
STATE: LOITER
[ INFO] [1520382319.997690808]: GUIDED enabled

Diagnostics

header: 
  seq: 18
  stamp: 
    secs: 1520382826
    nsecs: 209598083
  frame_id: ''
status: 
  - 
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: /dev/ttyACM0:115200
    values: 
      - 
        key: Received packets:
        value: 31
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 22
      - 
        key: Tx sequence number:
        value: 70
      - 
        key: Rx total bytes:
        value: 1201
      - 
        key: Tx total bytes:
        value: 4244
      - 
        key: Rx speed:
        value: 145.000000
      - 
        key: Tx speed:
        value: 688.000000
  - 
    level: 2
    name: mavros: GPS
    message: No satellites
    hardware_id: /dev/ttyACM0:115200
    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/ttyACM0:115200
    values: 
      - 
        key: Heartbeats since startup
        value: 6
      - 
        key: Frequency (Hz)
        value: 0.748047
      - 
        key: Vehicle type
        value: Quadrotor
      - 
        key: Autopilot type
        value: ArduPilotMega / ArduCopter
      - 
        key: Mode
        value: LOITER
      - 
        key: System status
        value: Standby
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/ttyACM0:115200
    values: 
      - 
        key: Sensor present
        value: 0x00000000
      - 
        key: Sensor enabled
        value: 0x00000000
      - 
        key: Sensor helth
        value: 0x00000000
      - 
        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: 2
    name: mavros: Battery
    message: No data
    hardware_id: /dev/ttyACM0:115200
    values: 
      - 
        key: Voltage
        value: -1.00
      - 
        key: Current
        value: 0.0
      - 
        key: Remaining
        value: 0.0

Check ID

[rospack] Error: package 'mavros' not found

My Code

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;
    ROS_INFO("Offboard node started");
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(10.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
    //ROS_INFO("waiting for connection");
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "GUIDED_NOGPS";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();
    std::cout << offb_set_mode.request.custom_mode << std::endl;
    std::cout << current_state.mode << std::endl;
    while(ros::ok()){
        if( current_state.mode != "GUIDED_NOGPS" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
        std::cout << "STATE: " << current_state.mode << std::endl;
                ROS_INFO("GUIDED enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}
alvikag commented 6 years ago

Hi, I have mavros 0.23.3 and ROS kinetic setup on an odroid which is communicating to 3DR solo over WiFi. I am not able to change the mode to GUIDED it just gets stuck in LOITER or results ina time out. If I try the same commands with mavros 0.21.2, mode change works perfectly.

vooon commented 6 years ago

@alvikag change config to set ~fcu_protocol="v1.0".

alvikag commented 6 years ago

Thankyou it worked!!