weixin_39611820
2020-12-27 18:00 阅读 4

simple script using mavros setpoint_position

I'm new to mavros. I am trying to use the simple takeoff script as seen here on the px4 docs work with the APM flight stack. I changed the "OFFBOARD" mode to the APM "GUIDED" mode, but every time I run the script the drone arms, but doesn't takeoff. I'm testing with STIL and gazebo. I can't seem to figure out what I'm doing wrong. Thanks in advance!

here is my code



/**
 *  nedTest.cpp
 */

#include <fstream>

#include <ros>
#include <mavros_msgs>
#include <geometry_msgs>
#include <mavros_msgs>
#include <mavros_msgs>

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::Subscriber state_sub = nh.subscribe<:state>("mavros/state", 1, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<:posestamped>("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<:commandbool>("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<:setmode>("mavros/set_mode");

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

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        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";

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

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
      //std::cout << pose << std::endl;
        if( current_state.mode != "GUIDED" && (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode)){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed && current_state.mode == "GUIDED" && (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;
}

</:setmode></:commandbool></:posestamped></:state></mavros_msgs></mavros_msgs></geometry_msgs></mavros_msgs></ros></fstream>

MAVROS version and platform

Mavros: release ROS: Kinetic Ubuntu: 16.04

Autopilot type and version

[ X ] ArduPilot [ ] PX4

Version: 3.5.4

Node logs


roslaunch apm.launch 
... logging to /home/eric/.ros/log/c14b89a2-e863-11e7-9c08-00212f318f2a/roslaunch-eric-MS-7971-28661.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://eric-MS-7971:43701/

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_url: udp://127.0.0.1:1...
 * /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)

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

setting /run_id to 5178add2-e866-11e7-9c08-00212f318f2a
process[rosout-1]: started with pid [31151]
started core service [/rosout]
process[mavros-2]: started with pid [31169]
[ INFO] [1514091181.408370893]: FCU URL: udp://127.0.0.1:14551
[ WARN] [1514091181.409130530]: init: message from ASLUAV, MSG-ID 201 ignored! Table has different entry.
[ WARN] [1514091181.409294253]: init: message from autoquad, MSG-ID 150 ignored! Table has different entry.
[ WARN] [1514091181.409316991]: init: message from autoquad, MSG-ID 152 ignored! Table has different entry.
[ WARN] [1514091181.409477281]: init: message from matrixpilot, MSG-ID 150 ignored! Table has different entry.
[ WARN] [1514091181.409497565]: init: message from matrixpilot, MSG-ID 151 ignored! Table has different entry.
[ WARN] [1514091181.409504093]: init: message from matrixpilot, MSG-ID 152 ignored! Table has different entry.
[ WARN] [1514091181.409523084]: init: message from matrixpilot, MSG-ID 153 ignored! Table has different entry.
[ WARN] [1514091181.409557837]: init: message from matrixpilot, MSG-ID 155 ignored! Table has different entry.
[ WARN] [1514091181.409571617]: init: message from matrixpilot, MSG-ID 156 ignored! Table has different entry.
[ WARN] [1514091181.409587509]: init: message from matrixpilot, MSG-ID 157 ignored! Table has different entry.
[ WARN] [1514091181.409622083]: init: message from matrixpilot, MSG-ID 158 ignored! Table has different entry.
[ WARN] [1514091181.409647934]: init: message from matrixpilot, MSG-ID 170 ignored! Table has different entry.
[ WARN] [1514091181.409660194]: init: message from matrixpilot, MSG-ID 171 ignored! Table has different entry.
[ WARN] [1514091181.409685825]: init: message from matrixpilot, MSG-ID 172 ignored! Table has different entry.
[ WARN] [1514091181.409698916]: init: message from matrixpilot, MSG-ID 173 ignored! Table has different entry.
[ WARN] [1514091181.409725022]: init: message from matrixpilot, MSG-ID 174 ignored! Table has different entry.
[ WARN] [1514091181.409737886]: init: message from matrixpilot, MSG-ID 175 ignored! Table has different entry.
[ WARN] [1514091181.409750836]: init: message from matrixpilot, MSG-ID 176 ignored! Table has different entry.
[ WARN] [1514091181.409763970]: init: message from matrixpilot, MSG-ID 177 ignored! Table has different entry.
[ WARN] [1514091181.409777024]: init: message from matrixpilot, MSG-ID 178 ignored! Table has different entry.
[ WARN] [1514091181.409789982]: init: message from matrixpilot, MSG-ID 179 ignored! Table has different entry.
[ WARN] [1514091181.409802805]: init: message from matrixpilot, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1514091181.409815872]: init: message from matrixpilot, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1514091181.409828771]: init: message from matrixpilot, MSG-ID 182 ignored! Table has different entry.
[ WARN] [1514091181.409841596]: init: message from matrixpilot, MSG-ID 183 ignored! Table has different entry.
[ WARN] [1514091181.409854476]: init: message from matrixpilot, MSG-ID 184 ignored! Table has different entry.
[ WARN] [1514091181.409867686]: init: message from matrixpilot, MSG-ID 185 ignored! Table has different entry.
[ WARN] [1514091181.409880968]: init: message from matrixpilot, MSG-ID 186 ignored! Table has different entry.
[ WARN] [1514091181.410029734]: init: message from paparazzi, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1514091181.410044301]: init: message from paparazzi, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1514091181.410057854]: init: message from paparazzi, MSG-ID 182 ignored! Table has different entry.
[ WARN] [1514091181.410083645]: init: message from paparazzi, MSG-ID 183 ignored! Table has different entry.
[ WARN] [1514091181.410096586]: init: message from paparazzi, MSG-ID 184 ignored! Table has different entry.
[ WARN] [1514091181.410211604]: init: message from slugs, MSG-ID 170 ignored! Table has different entry.
[ WARN] [1514091181.410225171]: init: message from slugs, MSG-ID 172 ignored! Table has different entry.
[ WARN] [1514091181.410238171]: init: message from slugs, MSG-ID 173 ignored! Table has different entry.
[ WARN] [1514091181.410251173]: init: message from slugs, MSG-ID 176 ignored! Table has different entry.
[ WARN] [1514091181.410262811]: init: message from slugs, MSG-ID 177 ignored! Table has different entry.
[ WARN] [1514091181.410274445]: init: message from slugs, MSG-ID 179 ignored! Table has different entry.
[ WARN] [1514091181.410286011]: init: message from slugs, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1514091181.410297863]: init: message from slugs, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1514091181.410309476]: init: message from slugs, MSG-ID 184 ignored! Table has different entry.
[ WARN] [1514091181.410321451]: init: message from slugs, MSG-ID 185 ignored! Table has different entry.
[ WARN] [1514091181.410333443]: init: message from slugs, MSG-ID 186 ignored! Table has different entry.
[ WARN] [1514091181.410350470]: init: message from slugs, MSG-ID 188 ignored! Table has different entry.
[ WARN] [1514091181.410380998]: init: message from slugs, MSG-ID 191 ignored! Table has different entry.
[ WARN] [1514091181.410397608]: init: message from slugs, MSG-ID 192 ignored! Table has different entry.
[ WARN] [1514091181.410425637]: init: message from slugs, MSG-ID 193 ignored! Table has different entry.
[ WARN] [1514091181.410440728]: init: message from slugs, MSG-ID 194 ignored! Table has different entry.
[ INFO] [1514091181.410929384]: udp0: Bind address: 127.0.0.1:14551
[ INFO] [1514091181.410987638]: udp0: Remote address: 0.0.56.219:14550
[ INFO] [1514091181.411183883]: GCS bridge disabled
[ INFO] [1514091181.420020755]: Plugin 3dr_radio loaded
[ INFO] [1514091181.422161516]: Plugin 3dr_radio initialized
[ INFO] [1514091181.422214486]: Plugin actuator_control blacklisted
[ INFO] [1514091181.424016614]: Plugin adsb loaded
[ INFO] [1514091181.426868730]: Plugin adsb initialized
[ INFO] [1514091181.426915265]: Plugin altitude blacklisted
[ INFO] [1514091181.427006488]: Plugin cam_imu_sync loaded
[ INFO] [1514091181.427662971]: Plugin cam_imu_sync initialized
[ INFO] [1514091181.427815403]: Plugin command loaded
[ INFO] [1514091181.433198376]: Plugin command initialized
[ INFO] [1514091181.433249016]: Plugin debug_value blacklisted
[ INFO] [1514091181.433384063]: Plugin distance_sensor loaded
[ INFO] [1514091181.442067529]: Plugin distance_sensor initialized
[ INFO] [1514091181.442222337]: Plugin fake_gps loaded
[ INFO] [1514091181.453644979]: Plugin fake_gps initialized
[ INFO] [1514091181.453699863]: Plugin ftp blacklisted
[ INFO] [1514091181.453830411]: Plugin global_position loaded
[ INFO] [1514091181.461281350]: udp0: Remote address: 127.0.0.1:47526
[ INFO] [1514091181.474788994]: Plugin global_position initialized
[ INFO] [1514091181.474922698]: Plugin hil loaded
[ INFO] [1514091181.486756599]: Plugin hil initialized
[ INFO] [1514091181.486887826]: Plugin home_position loaded
[ INFO] [1514091181.490017329]: Plugin home_position initialized
[ INFO] [1514091181.490129940]: Plugin imu loaded
[ INFO] [1514091181.495951237]: Plugin imu initialized
[ INFO] [1514091181.496079342]: Plugin local_position loaded
[ INFO] [1514091181.501311397]: Plugin local_position initialized
[ INFO] [1514091181.501440175]: Plugin manual_control loaded
[ INFO] [1514091181.502437548]: Plugin manual_control initialized
[ INFO] [1514091181.502463052]: Plugin mocap_pose_estimate blacklisted
[ INFO] [1514091181.502563925]: Plugin odom loaded
[ INFO] [1514091181.506278869]: Plugin odom initialized
[ INFO] [1514091181.506468448]: Plugin param loaded
[ INFO] [1514091181.508789403]: Plugin param initialized
[ INFO] [1514091181.508814042]: Plugin px4flow blacklisted
[ INFO] [1514091181.508912597]: Plugin rangefinder loaded
[ INFO] [1514091181.509535240]: Plugin rangefinder initialized
[ INFO] [1514091181.509676282]: Plugin rc_io loaded
[ INFO] [1514091181.513238444]: Plugin rc_io initialized
[ INFO] [1514091181.513263441]: Plugin safety_area blacklisted
[ INFO] [1514091181.513375138]: Plugin setpoint_accel loaded
[ INFO] [1514091181.516007941]: Plugin setpoint_accel initialized
[ INFO] [1514091181.516191627]: Plugin setpoint_attitude loaded
[ INFO] [1514091181.526725157]: Plugin setpoint_attitude initialized
[ INFO] [1514091181.526866165]: Plugin setpoint_position loaded
[ INFO] [1514091181.543565964]: Plugin setpoint_position initialized
[ INFO] [1514091181.543708426]: Plugin setpoint_raw loaded
[ INFO] [1514091181.551142042]: Plugin setpoint_raw initialized
[ INFO] [1514091181.551265570]: Plugin setpoint_velocity loaded
[ INFO] [1514091181.560232563]: Plugin setpoint_velocity initialized
[ INFO] [1514091181.560587841]: Plugin sys_status loaded
[ INFO] [1514091181.570919583]: Plugin sys_status initialized
[ INFO] [1514091181.571090342]: Plugin sys_time loaded
[ INFO] [1514091181.573944649]: TM: Timesync mode: NONE
[ INFO] [1514091181.574552945]: Plugin sys_time initialized
[ INFO] [1514091181.574694467]: Plugin vfr_hud loaded
[ INFO] [1514091181.575915227]: Plugin vfr_hud initialized
[ INFO] [1514091181.575966033]: Plugin vibration blacklisted
[ INFO] [1514091181.575991023]: Plugin vision_pose_estimate blacklisted
[ INFO] [1514091181.576017836]: Plugin vision_speed_estimate blacklisted
[ INFO] [1514091181.576147765]: Plugin waypoint loaded
[ INFO] [1514091181.580251399]: Plugin waypoint initialized
[ INFO] [1514091181.580334222]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1514091181.580361531]: Built-in MAVLink package version: 2017.12.1
[ INFO] [1514091181.580387233]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1514091181.580410761]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ WARN] [1514091181.657217471]: IMU: linear acceleration on RAW_IMU known on APM only.
[ WARN] [1514091181.657256528]: IMU: ~imu/data_raw stores unscaled raw acceleration report.
[ INFO] [1514091181.660653100]: RC_CHANNELS message detected!
[ INFO] [1514091181.758957057]: CON: Got HEARTBEAT, connected. FCU: ArduPilotMega / ArduCopter
[ INFO] [1514091181.874812828]: RC_CHANNELS message detected!
[ INFO] [1514091182.780415058]: VER: 1.1: Capabilities         0x0000000000001bcf
[ INFO] [1514091182.780694748]: VER: 1.1: Flight software:     03060000 ( be96a27)
[ INFO] [1514091182.780923828]: VER: 1.1: Middleware software: 00000000 (        )
[ INFO] [1514091182.781055754]: VER: 1.1: OS software:         00000000 (        )
[ INFO] [1514091182.781179091]: VER: 1.1: Board hardware:      00000000
[ INFO] [1514091182.781295174]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1514091182.781408510]: VER: 1.1: UID:                 0000000000000000
[ WARN] [1514091182.781691187]: CMD: Unexpected command 520, result 0
[ INFO] [1514091191.760165720]: HP: requesting home position
[ INFO] [1514091191.763094896]: FCU: APM:Copter V3.6-dev (be96a27f)
[ INFO] [1514091191.763575407]: FCU: Frame: QUAD
[ WARN] [1514091192.761676923]: PR: request list timeout, retries left 2
[ WARN] [1514091193.761982490]: PR: request list timeout, retries left 1
[ WARN] [1514091194.762528730]: PR: request list timeout, retries left 0
[ WARN] [1514091196.763366459]: CMD: Unexpected command 410, result 0
[ INFO] [1514091196.791075549]: FCU: APM:Copter V3.6-dev (be96a27f)
[ INFO] [1514091196.791226011]: FCU: Frame: QUAD
[ INFO] [1514091196.791300058]: FCU: APM:Copter V3.6-dev (be96a27f)
[ INFO] [1514091196.791350508]: FCU: Frame: QUAD
[ INFO] [1514091196.791398027]: FCU: APM:Copter V3.6-dev (be96a27f)
[ INFO] [1514091196.791443874]: FCU: Frame: QUAD
[ INFO] [1514091196.791630474]: WP: mission received
[ WARN] [1514091197.207504116]: PR: Param STAT_FLTTIME (65535/897): <value><i4>231</i4></value> different index: 704/897
[ WARN] [1514091197.207772146]: PR: Param STAT_RUNTIME (65535/897): <value><i4>61265</i4></value> different index: 705/897
[ INFO] [1514091197.241270773]: PR: parameters list received
[ INFO] [1514091334.992763785]: FCU: Arming motors
[ INFO] [1514091344.995729407]: FCU: Disarming motors
[ INFO] [1514091345.477292401]: FCU: Arming motors
[ INFO] [1514091355.602757126]: FCU: Disarming motors

Diagnostics


header: 
  seq: 18
  stamp: 
    secs: 1514091199
    nsecs: 581723004
  frame_id: ''
status: 
  - 
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "udp://127.0.0.1:14551"
    values: 
      - 
        key: "Received packets:"
        value: "2965"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "80"
      - 
        key: "Tx sequence number:"
        value: "26"
      - 
        key: "Rx total bytes:"
        value: "94563"
      - 
        key: "Tx total bytes:"
        value: "449"
      - 
        key: "Rx speed:"
        value: "3764.000000"
      - 
        key: "Tx speed:"
        value: "17.000000"
  - 
    level: 0
    name: "mavros: GPS"
    message: "3D fix"
    hardware_id: "udp://127.0.0.1:14551"
    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: "mavros: Heartbeat"
    message: "Normal"
    hardware_id: "udp://127.0.0.1:14551"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "14"
      - 
        key: "Frequency (Hz)"
        value: "0.692304"
      - 
        key: "Vehicle type"
        value: "Quadrotor"
      - 
        key: "Autopilot type"
        value: "ArduPilotMega / ArduCopter"
      - 
        key: "Mode"
        value: "GUIDED"
      - 
        key: "System status"
        value: "Standby"
  - 
    level: 0
    name: "mavros: System"
    message: "Normal"
    hardware_id: "udp://127.0.0.1:14551"
    values: 
      - 
        key: "Sensor present"
        value: "0x0361FDEF"
      - 
        key: "Sensor enabled"
        value: "0x0261FDEF"
      - 
        key: "Sensor helth"
        value: "0x0361FDEF"
      - 
        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: "optical flow"
        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: "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: "mavros: Battery"
    message: "Normal"
    hardware_id: "udp://127.0.0.1:14551"
    values: 
      - 
        key: "Voltage"
        value: "12.59"
      - 
        key: "Current"
        value: "0.0"
      - 
        key: "Remaining"
        value: "93.0"
---

Check ID

OK. I got messages from 1:1.

Received 1643 messages, from 1 addresses sys:comp list of messages 1:1 0, 1, 2, 132, 65, 136, 152, 147, 150, 24, 27, 29, 30, 32, 33, 163, 36, 165, 42, 173, 178, 182, 62, 193, 74, 35, 164, 100, 241, 116, 125

该提问来源于开源项目:mavlink/mavros

  • 点赞
  • 写回答
  • 关注问题
  • 收藏
  • 复制链接分享

7条回答 默认 最新

  • weixin_39559750 weixin_39559750 2020-12-27 18:00

    maybe you can lend a hand here?

    点赞 评论 复制链接分享
  • weixin_39559750 weixin_39559750 2020-12-27 18:00

    have you found the solution?

    点赞 评论 复制链接分享
  • weixin_39611820 weixin_39611820 2020-12-27 18:00

    So here is what my team and I have learned. We noticed in a previous issue that you cannot issue setpoint position commands when you are on the ground, even though in the px4 tutorial they can get it to takeoff with setpoint position(i assume a px4 flight stack difference). Once we had the drone takeoff we were able to have it move to the destinations we desired. Some things that we have noticed about this command is that the drone sometimes moves very slowly like the FCU is being overwhelmed by messages. We think this because if we break our script the drone will complete the maneuver at a more reasonable pace. We were also interested in using setpoint_raw command so we can control the speed at which the drone moves, but with the limited testing we have done so far the drone seems to continue past the desired position. Is this command supposed to work the same as its setpoint position counter part?

    点赞 评论 复制链接分享
  • weixin_39559750 weixin_39559750 2020-12-27 18:00

    We were also interested in using setpoint_raw command so we can control the speed at which the drone moves, but with the limited testing we have done so far the drone seems to continue past the desired position. Is this command supposed to work the same as its setpoint position counter part?

    This is all a matter of how you are generating the commands and how the autopilot controller handles them. Closing this now.

    点赞 评论 复制链接分享
  • weixin_40007804 weixin_40007804 2020-12-27 18:00

    So here is what my team and I have learned. We noticed in a previous issue that you cannot issue setpoint position commands when you are on the ground, even though in the px4 tutorial they can get it to takeoff with setpoint position(i assume a px4 flight stack difference). Once we had the drone takeoff we were able to have it move to the destinations we desired. Some things that we have noticed about this command is that the drone sometimes moves very slowly like the FCU is being overwhelmed by messages. We think this because if we break our script the drone will complete the maneuver at a more reasonable pace. We were also interested in using setpoint_raw command so we can control the speed at which the drone moves, but with the limited testing we have done so far the drone seems to continue past the desired position. Is this command supposed to work the same as its setpoint position counter part?

    I have the same issue as you mentioned in this post. Could you please tell me how you managed to solve this issue? Thanks!

    点赞 评论 复制链接分享
  • weixin_39611820 weixin_39611820 2020-12-27 18:00

    to get a quad to takeoff using arducopter you need to use the takeoff service. After the drone is in the air you can then use the setpoint_position topic. here is some example code for a takeoff sequence.

    
    //intitialize first waypoint of mission
        set_destination(0,0,takeoff_alt,0);
        for(int i=0; i<100; i++)
        {
            local_pos_pub.publish(waypoint_g);
            ros::spinOnce();
            ros::Duration(0.01).sleep();
        }
        // arming
        ROS_INFO("Arming drone");
        mavros_msgs::CommandBool arm_request;
        arm_request.request.value = true;
        while (!current_state_g.armed && !arm_request.response.success)
        {
            ros::Duration(.1).sleep();
            arming_client.call(arm_request);
            local_pos_pub.publish(waypoint_g);
        }
        if(arm_request.response.success)
        {
            ROS_INFO("Arming Successful");  
        }else{
            ROS_INFO("Arming failed with %d", arm_request.response.success);
            return -1;  
        }
    
        //request takeoff
    
        mavros_msgs::CommandTOL srv_takeoff;
        srv_takeoff.request.altitude = takeoff_alt;
        if(takeoff_client.call(srv_takeoff)){
            ROS_INFO("takeoff sent %d", srv_takeoff.response.success);
        }else{
            ROS_ERROR("Failed Takeoff");
            return -2;
        }
        sleep(5);
        return 0; 
    

    My team is working on documenting our projects, so this link might be helpful.

    The control API tutorial isn't finished, but I will try to update it tomorrow.

    here is some more example code

    As for the sluggish movement, we reduced the rate we are publishing the waypoint to 2hz. We also noticed that you can control speed, acceleration, ect by using the WP_NAV parameters in arducopter.

    Feel free to message me with questions. I know that when we were learning mavros, the task was pretty overwhelming.

    点赞 评论 复制链接分享
  • weixin_40007804 weixin_40007804 2020-12-27 18:00

    Thank you for the reply! Right now I can make the quad takeoff to the sky, however, when I publish to the /mavros/setpoint_position/local topic, the quad doesn't change its path. It just keeps flying upward.

    Here is the code I have (I'm writing in Python).

    
    def setLocation():
        locPub = rospy.Publisher('/mavros/setpoint_position/local',PoseStamped,queue_size=10)
        loop_rate = rospy.Rate(10)
    
        while not rospy.is_shutdown():
            goalPos = PoseStamped()
            goalPos.pose.position.x = 0
            goalPos.pose.position.y = 0
            goalPos.pose.position.z = 15
            goalPos.header.stamp = rospy.Time.now()
            goalPos.header.frame_id = 'base_link'
    
            locPub.publish(goalPos)
            loop_rate.sleep()
    

    So basically after the quad successfully takeoff, I just call the setLocation() function. But it doesn't work. Do you have any idea what might cause this issue? Thanks!

    点赞 评论 复制链接分享