search

mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
879 stars 990 forks source link

Mode change using MAVROS #1025

Closed trishantroy closed 6 years ago

trishantroy commented 6 years ago

Issue details

I am using rc/override to control the quad. I am unable to change mode of the quad (stabilise to loiter) through code. I have tried using setMode rosservice. Can setMode be used for APM flight stack?

    ros::ServiceClient cl = n.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
    mavros_msgs::SetMode srv_setMode;
    srv_setMode.request.base_mode = 0;
    srv_setMode.request.custom_mode = "ALT_HOLD";
    if(cl.call(srv_setMode)){
        ROS_INFO("setmode send ok %d value:", srv_setMode.response.mode_sent);
    }else{
        ROS_INFO("Failed SetMode");
    }

MAVROS version and platform

Mavros: 0.24.0 ROS: Kinetic Ubuntu: 16.04

Autopilot type and version

APM 3.5.5

Node logs

copy output of mavros_node. Usually console where you run roslaunch

... logging to /home/iarc/.ros/log/0cebc4da-5399-11e8-b159-a434d90f8f75/roslaunch-iarc-nuc-2124.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://iarc-nuc.local:39917/

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:57600
 * /mavros/gcs_url: 
 * /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.12

NODES
  /
    mavros (mavros/mavros_node)

ROS_MASTER_URI=http://sine-ThinkPad-P50.local:11311

process[mavros-1]: started with pid [2133]
[ INFO] [1525877749.352189167]: FCU URL: /dev/ttyACM0:57600
[ INFO] [1525877749.359316256]: serial0: device: /dev/ttyACM0 @ 57600 bps
[ INFO] [1525877749.360949191]: GCS bridge disabled
[ INFO] [1525877749.529338847]: Plugin 3dr_radio loaded
[ INFO] [1525877749.586081514]: Plugin 3dr_radio initialized
[ INFO] [1525877749.586194329]: Plugin actuator_control blacklisted
[ INFO] [1525877749.605363979]: Plugin adsb loaded
[ INFO] [1525877749.744211740]: Plugin adsb initialized
[ INFO] [1525877749.744367555]: Plugin altitude blacklisted
[ INFO] [1525877749.744830734]: Plugin cam_imu_sync loaded
[ INFO] [1525877749.761923841]: Plugin cam_imu_sync initialized
[ INFO] [1525877749.762568193]: Plugin command loaded
[ INFO] [1525877749.912200070]: Plugin command initialized
[ INFO] [1525877749.912309397]: Plugin debug_value blacklisted
[ INFO] [1525877749.912843415]: Plugin distance_sensor loaded
[ INFO] [1525877750.284735299]: Plugin distance_sensor initialized
[ INFO] [1525877750.285282547]: Plugin fake_gps loaded
[ INFO] [1525877750.906830911]: Plugin fake_gps initialized
[ INFO] [1525877750.906941326]: Plugin ftp blacklisted
[ INFO] [1525877750.907449094]: Plugin global_position loaded
[ INFO] [1525877751.631752604]: Plugin global_position initialized
[ INFO] [1525877751.631866512]: Plugin hil blacklisted
[ INFO] [1525877751.632374520]: Plugin home_position loaded
[ INFO] [1525877751.793067452]: Plugin home_position initialized
[ INFO] [1525877751.793576803]: Plugin imu loaded
[ INFO] [1525877752.044039750]: Plugin imu initialized
[ INFO] [1525877752.044562652]: Plugin local_position loaded
[ INFO] [1525877752.266049715]: Plugin local_position initialized
[ INFO] [1525877752.266702607]: Plugin manual_control loaded
[ INFO] [1525877752.400320744]: Plugin manual_control initialized
[ INFO] [1525877752.400426213]: Plugin mocap_pose_estimate blacklisted
[ INFO] [1525877752.400953390]: Plugin obstacle_distance loaded
[ INFO] [1525877752.485707599]: Plugin obstacle_distance initialized
[ INFO] [1525877752.486254061]: Plugin odom loaded
[ INFO] [1525877752.660448383]: Plugin odom initialized
[ INFO] [1525877752.661286152]: Plugin param loaded
[ INFO] [1525877752.736327909]: Plugin param initialized
[ INFO] [1525877752.736433398]: Plugin px4flow blacklisted
[ INFO] [1525877752.736911927]: Plugin rangefinder loaded
[ INFO] [1525877752.753258465]: Plugin rangefinder initialized
[ INFO] [1525877752.753885398]: Plugin rc_io loaded
[ INFO] [1525877752.893115411]: Plugin rc_io initialized
[ INFO] [1525877752.893213823]: Plugin safety_area blacklisted
[ INFO] [1525877752.893777539]: Plugin setpoint_accel loaded
[ INFO] [1525877753.036969890]: Plugin setpoint_accel initialized
[ INFO] [1525877753.038379549]: Plugin setpoint_attitude loaded
[ INFO] [1525877753.443206399]: Plugin setpoint_attitude initialized
[ INFO] [1525877753.443485035]: Plugin setpoint_position loaded
[ INFO] [1525877753.996755677]: Plugin setpoint_position initialized
[ INFO] [1525877753.997427308]: Plugin setpoint_raw loaded
[ INFO] [1525877754.360878270]: Plugin setpoint_raw initialized
[ INFO] [1525877754.361451283]: Plugin setpoint_velocity loaded
[ INFO] [1525877754.580108919]: Plugin setpoint_velocity initialized
[ INFO] [1525877754.580993014]: Plugin sys_status loaded
[ INFO] [1525877754.900160944]: Plugin sys_status initialized
[ INFO] [1525877754.900752916]: Plugin sys_time loaded
[ INFO] [1525877755.021606410]: TM: Timesync mode: NONE
[ INFO] [1525877755.038044835]: Plugin sys_time initialized
[ INFO] [1525877755.038627731]: Plugin vfr_hud loaded
[ INFO] [1525877755.078082657]: Plugin vfr_hud initialized
[ INFO] [1525877755.078197867]: Plugin vibration blacklisted
[ INFO] [1525877755.078701535]: Plugin vision_pose_estimate loaded
[ INFO] [1525877755.421817417]: Plugin vision_pose_estimate initialized
[ INFO] [1525877755.421988917]: Plugin vision_speed_estimate blacklisted
[ INFO] [1525877755.422593739]: Plugin waypoint loaded
[ INFO] [1525877755.559368123]: Plugin waypoint initialized
[ INFO] [1525877755.559621342]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1525877755.559730775]: Built-in MAVLink package version: 2018.4.4
[ INFO] [1525877755.559847970]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad icarous matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1525877755.559965081]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ERROR] [1525877755.967063598]: FCU: PreArm: Gyros not calibrated
[ERROR] [1525877755.967534751]: FCU: PreArm: Check Board Voltage
[ERROR] [1525877755.967817439]: FCU: PreArm: check range finder
[ INFO] [1525877755.969302833]: CON: Got HEARTBEAT, connected. FCU: ArduPilotMega / ArduCopter
[ INFO] [1525877757.030679048]: VER: 1.1: Capabilities         0x0000000000001bcf
[ INFO] [1525877757.030800624]: VER: 1.1: Flight software:     030505ff (27229c830384802e1bcae90b)
[ INFO] [1525877757.030929476]: VER: 1.1: Middleware software: 00000000 (0384802e1bcae90b)
[ INFO] [1525877757.031043650]: VER: 1.1: OS software:         00000000 (1bcae90b)
[ INFO] [1525877757.031148612]: VER: 1.1: Board hardware:      00000000
[ INFO] [1525877757.031263598]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1525877757.031387890]: VER: 1.1: UID:                 0000000000000000
[ WARN] [1525877757.031699229]: CMD: Unexpected command 520, result 0
[ INFO] [1525877765.970425681]: HP: requesting home position
[ INFO] [1525877765.973481396]: FCU: APM:Copter V3.5.5 (27229c83)
[ INFO] [1525877765.973802803]: FCU: PX4: 0384802e NuttX: 1bcae90b
[ INFO] [1525877765.974001461]: FCU: Frame: QUAD
[ INFO] [1525877765.974221925]: FCU: PX4v2 003B0025 3236510F 37353639
[ INFO] [1525877770.974704903]: WP: mission received
[ INFO] [1525877773.401677405]: PR: parameters list received
[ INFO] [1525877775.970024035]: HP: requesting home position
[ INFO] [1525877785.970662450]: HP: requesting home position
[ERROR] [1525877785.978848824]: FCU: PreArm: Gyros not calibrated
[ERROR] [1525877785.979159913]: FCU: PreArm: Check Board Voltage
[ERROR] [1525877785.979391176]: FCU: PreArm: check range finder
[ I

Diagnostics

place here result of:
rostopic echo -n1 /diagnostics

header: 
  seq: 82
  stamp: 
    secs: 1525877860
    nsecs: 561317783
  frame_id: ''
status: 
  - 
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "/dev/ttyACM0:57600"
    values: 
      - 
        key: "Received packets:"
        value: "1991"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "195"
      - 
        key: "Tx sequence number:"
        value: "119"
      - 
        key: "Rx total bytes:"
        value: "67126"
      - 
        key: "Tx total bytes:"
        value: "2729"
      - 
        key: "Rx speed:"
        value: "305.000000"
      - 
        key: "Tx speed:"
        value: "21.000000"
  - 
    level: 2
    name: "mavros: GPS"
    message: "No satellites"
    hardware_id: "/dev/ttyACM0: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/ttyACM0:57600"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "105"
      - 
        key: "Frequency (Hz)"
        value: "0.962936"
      - 
        key: "Vehicle type"
        value: "Quadrotor"
      - 
        key: "Autopilot type"
        value: "ArduPilotMega / ArduCopter"
      - 
        key: "Mode"
        value: "STABILIZE"
      - 
        key: "System status"
        value: "Standby"
  - 
    level: 0
    name: "mavros: System"
    message: "Normal"
    hardware_id: "/dev/ttyACM0:57600"
    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:57600"
    values: 
      - 
        key: "Voltage"
        value: "-1.00"
      - 
        key: "Current"
        value: "0.0"
      - 
        key: "Remaining"
        value: "0.0"
---

Check ID

rosrun mavros checkid

OK. I got messages from 1:1.

---
Received 139 messages, from 1 addresses
sys:comp   list of messages
  1:1     0, 74, 77, 178, 182, 253, 30
trishantroy commented 6 years ago

Is there a way to check the current mode?

Thanks in advance.

lamping7 commented 6 years ago

mavros/state -- http://wiki.ros.org/mavros#mavros.2BAC8-Plugins.sys_status