danividanivi commented 6 years ago

Issue details

I am trying to replicate the offboard control example (which is working fine in gazebo) but sending global coordinates instead of local x,y,z. I am unable to do so, drone enters a loop of "Offboard Enabled" but never goes into Vehicle Armed or moves at all.

This is the code I am using:

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/GlobalPositionTarget.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::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::Publisher global_pos_pub = nh.advertise<mavros_msgs::GlobalPositionTarget>
            ("mavros/setpoint_position/global", 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(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;

    mavros_msgs::GlobalPositionTarget target;
    target.latitude = 42.3005867;
    target.longitude = -6.73246288;
    target.altitude = 288;

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

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

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

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

    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard 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);
        global_pos_pub.publish(target);

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

    return 0;
}

MAVROS version and platform

Mavros: 0.22 ROS: Kinetic Ubuntu: 16.04

Autopilot type and version

[ ] ArduPilot [X] PX4

Version: 1.72

Node logs

[ INFO] [1513871636.932979892, 18.704000000]: Offboard enabled
[ INFO] [1513871642.007625263, 23.756000000]: Offboard enabled
[ INFO] [1513871647.086730559, 28.802000000]: Offboard enabled
[ INFO] [1513871652.230028795, 33.854000000]: Offboard enabled
[ INFO] [1513871657.324560463, 38.904000000]: Offboard enabled
[ INFO] [1513871662.405701045, 43.952000000]: Offboard enabled
[ INFO] [1513871667.482897607, 49.002000000]: Offboard enabled
[ INFO] [1513871672.582290375, 54.054000000]: Offboard enabled
[ INFO] [1513871677.688265367, 59.104000000]: Offboard enabled
[ INFO] [1513871682.756627458, 64.152000000]: Offboard enabled
[ INFO] [1513871687.861573086, 69.204000000]: Offboard enabled
[ INFO] [1513871692.928765643, 74.252000000]: Offboard enabled
[ INFO] [1513871698.006964586, 79.304000000]: Offboard enabled
[ INFO] [1513871703.113995111, 84.354000000]: Offboard enabled
[ INFO] [1513871708.205142370, 89.404000000]: Offboard enabled
[ INFO] [1513871713.332829113, 94.454000000]: Offboard enabled
[ INFO] [1513871718.443985089, 99.504000000]: Offboard enabled

Diagnostics

header: 
  seq: 58
  stamp: 
    secs: 53
    nsecs:         0
  frame_id: ''
status: 
  - 
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "udp://:14540@localhost:14557"
    values: 
      - 
        key: "Received packets:"
        value: "19548"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "94"
      - 
        key: "Tx sequence number:"
        value: "108"
      - 
        key: "Rx total bytes:"
        value: "808043"
      - 
        key: "Tx total bytes:"
        value: "26524"
      - 
        key: "Rx speed:"
        value: "15016.000000"
      - 
        key: "Tx speed:"
        value: "510.000000"
  - 
    level: 0
    name: "mavros: GPS"
    message: "3D fix"
    hardware_id: "udp://:14540@localhost:14557"
    values: 
      - 
        key: "Satellites visible"
        value: "10"
      - 
        key: "Fix type"
        value: "3"
      - 
        key: "EPH (m)"
        value: "0.00"
      - 
        key: "EPV (m)"
        value: "0.00"
  - 
    level: 0
    name: "mavros: Heartbeat"
    message: "Normal"
    hardware_id: "udp://:14540@localhost:14557"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "48"
      - 
        key: "Frequency (Hz)"
        value: "0.900000"
      - 
        key: "Vehicle type"
        value: "Quadrotor"
      - 
        key: "Autopilot type"
        value: "PX4 Autopilot"
      - 
        key: "Mode"
        value: "AUTO.LOITER"
      - 
        key: "System status"
        value: "Standby"
  - 
    level: 0
    name: "mavros: System"
    message: "Normal"
    hardware_id: "udp://:14540@localhost:14557"
    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: 0
    name: "mavros: Battery"
    message: "Normal"
    hardware_id: "udp://:14540@localhost:14557"
    values: 
      - 
        key: "Voltage"
        value: "12.15"
      - 
        key: "Current"
        value: "-1.0"
      - 
        key: "Remaining"
        value: "100.0"
  - 
    level: 2
    name: "mavros: Time Sync"
    message: "No events recorded."
    hardware_id: "udp://:14540@localhost:14557"
    values: 
      - 
        key: "Timesyncs since startup"
        value: "0"
      - 
        key: "Frequency (Hz)"
        value: "0.000000"
      - 
        key: "Last dt (ms)"
        value: "-9.905652"
      - 
        key: "Mean dt (ms)"
        value: "0.000000"
      - 
        key: "Last system time (s)"
        value: "48.740659000"
      - 
        key: "Time offset (s)"
        value: "4.147428739"

Check ID

OK. I got messages from 1:1.

---
Received 5370 messages, from 1 addresses
sys:comp   list of messages
  1:1     0, 1, 2, 140, 141, 147, 24, 30, 31, 32, 33, 74, 76, 77, 85, 87, 102, 230, 105, 111, 241, 242, 245

TSC21 commented 6 years ago

First, while(ros::ok() && current_state.connected){ should be while(ros::ok() && !current_state.connected){. Also - https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/setpoint_position.cpp#L69 - make sure you are getting data from the global_position/global topic.

danividanivi commented 6 years ago

I corrected the while loop with the ! rostopic echo global_position/global returns the correct output:

header: 
  seq: 283
  stamp: 
    secs: 193
    nsecs: 832815640
  frame_id: "base_link"
status: 
  status: 0
  service: 1
latitude: 42.3005867
longitude: -6.7324631
altitude: 2120.89452234
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 1

However, it is still not working. when I run the code it finishes without arming or enabling offboard. The only warning it produces is:

[ WARN] [1514991840.139563807, 7583.766000000]: SPG: sp not sent.
INFO  [tone_alarm] negative
[ WARN] [1514991840.303175629, 7583.928000000]: CMD: Unexpected command 176, result 1

danividanivi commented 6 years ago

I have to translate from global to local and then send in mavros/setpoint_position/local otherwise it won't work. I am curious, is it me the only one with this problem? Is someone able to reproduce the aircraft offboard example just by publishing in mavros/setpoint_position/global?

TSC21 commented 6 years ago

@mzahana can you please guide @danividanivi on the issue above? Thanks

oceanusxiv commented 6 years ago

I would just like to mention that I have also encountered this issue. I was able to confirm that I was publishing to the correct topic, but MAVROS does not seem to be able to translate it into a correct MAVLINK setpoint message for the PX4. I checked using QGroundcontrol and was only able to see a setpoint message being published on the PX4 when using local.

TSC21 commented 6 years ago

@mzahana can you restest your implementation and make sure it works? Thanks

TSC21 commented 6 years ago

@eric1221bday quick question: do you get GPS data on MAVROS? Also what do you mean with:

but MAVROS does not seem to be able to translate it into a correct MAVLINK setpoint message for the PX4

This sounds kinda vague and we can't debug any issue with information like this.

oceanusxiv commented 6 years ago

I was using SITL, and yes, GPS simulation was enabled and I was able to view the topic in ROS. As for the mavlink thing. What I meant was simply that it didn't seem like the global setpoint was being sent to the px4 correctly, since I was unable to see it using Mavlink Inspector.

TSC21 commented 6 years ago

This isn't supposed to publish any global position setpoint, but rather convert from global to local and publish as a local position setpoint. So you actually never see a global setpoint.

oceanusxiv commented 6 years ago

I was able to deduce that yes, and I was similarly unable to see any local setpoints either from the PX4.

TSC21 commented 6 years ago

Well that does depend on how your node is set and what you publish or not to MAVROS. This cannot be debugged without further info.

TSC21 commented 6 years ago

@eric1221bday anything else to add or can we close this?

mzahana commented 6 years ago

I will retest this on my side and get back to you.

pltmoreira commented 6 years ago

Any progress? I'm having the same issue and can't seem to find a solution.

tropappar commented 6 years ago

Make sure you set the header.stamp field. Only then it works!

DiegoSANFRAN commented 6 years ago

Good afternoon! Could anyone confirm that publishing to the topic /mavros/setpoint_position/global they hace successfully managed to control the drone’s position? I am having the same issue (no response obtained, px4 does not seem to receive any message) even setting the header.stamp field

tropappar commented 6 years ago

Yes, it works for me!

jukindle commented 6 years ago

Same here... (does not work for me)

aurashn commented 5 years ago

@tropappar do you mind sharing some sample code. I am running script similar to the one at the top, yet on rqt_graph I don’t see any topics being published from my node even though it is successfully publishing.

tropappar commented 5 years ago

Here you go:

#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/GlobalPositionTarget.h>
#include <sensor_msgs/NavSatFix.h>

// global variables
mavros_msgs::State current_state;
sensor_msgs::NavSatFix global_position;
bool global_position_received = false;

// callback functions
void globalPosition_cb(const sensor_msgs::NavSatFix::ConstPtr& msg) {
    global_position = *msg;
    global_position_received = true;
    ROS_INFO_ONCE("Got global position: [%.2f, %.2f, %.2f]", msg->latitude, msg->longitude, msg->altitude);
}

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

// main function
int main(int argc, char **argv) {
    ros::init(argc, argv, "test");
    ros::NodeHandle nh;

    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");
    ros::Subscriber state_sub = nh.subscribe < mavros_msgs::State > ("mavros/state", 10, state_cb);
    ros::Subscriber global_pos_sub = nh.subscribe < sensor_msgs::NavSatFix > ("mavros/global_position/global", 1, globalPosition_cb);
    ros::Publisher goal_pos_pub = nh.advertise < mavros_msgs::GlobalPositionTarget > ("mavros/setpoint_position/global", 10);

    ros::Rate rate(10);

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

    // wait for position information
    while (ros::ok() && !global_position_received) {
        ROS_INFO_ONCE("Waiting for GPS signal...");
        ros::spinOnce();
        rate.sleep();
    }
    ROS_INFO("GPS position received");

    // set target position
    mavros_msgs::GlobalPositionTarget goal_position;
    goal_position.latitude = global_position.latitude;
    goal_position.longitude = global_position.longitude;
    goal_position.altitude = global_position.altitude;

    // send a few setpoints before starting
    for (int i=0; i<20; ++i) {
        goal_position.header.stamp = ros::Time::now();
        goal_pos_pub.publish(goal_position);
        ros::spinOnce();
        rate.sleep();
    }

    // set mode
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.base_mode = 0;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
        ROS_INFO("OFFBOARD enabled");
    } else {
        ROS_ERROR("Failed to set OFFBOARD");
    }

    // arm
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
        ROS_INFO("Vehicle armed");
    } else {
        ROS_ERROR("Arming failed");
    }

    // take off to 5m above ground
    goal_position.altitude = goal_position.altitude + 5.0;
    while (ros::ok()) {
        goal_position.header.stamp = ros::Time::now();
        goal_pos_pub.publish(goal_position);
        ros::spinOnce();
        ROS_INFO_THROTTLE(1, "At altitude %.2f", global_position.altitude);
        rate.sleep();
    }

    return 0;
}

zxz0622 commented 5 years ago

Good afternoon! Could anyone confirm that publishing to the topic /mavros/setpoint_position/global they hace successfully managed to control the drone’s position? I am having the same issue (no response obtained, px4 does not seem to receive any message) even setting the header.stamp field

me too, but the topic /mavros/setpoint_raw/global has worked normally.

omeralierdemir commented 5 years ago

hi zxz0622, I've been working on /mavros/setpoint_raw/global for about 4 days but I couldn't move the quadcopter by giving the latitude longitude altitude value. Can you tell me how you do it please ? Can you share the parameters of the /mavros/setpoint_raw/global ? ( coordinate_frame ...)

swaroophangal commented 4 years ago

Sorry for digging up an old thread, but I am trying to do the same. I am unable to control the vehicle by sending position setpoints on _mavros/setpointposition/global. Vehicle arms, sits on the ground spinning rotors and disarms after timeout. I can confirm that rostopic echo /mavros/setpoint_position/global shows that setpoints from my code are being published. I can also confirm that sending local co-ordinate setpoints on _mavros/setpointposition/global works perfectly. Am I supposed to set any other field than lat, lon and lat?

MAVROS version and platform

Mavros: master ROS: Melodic Ubuntu: 18.04 Autopilot type and version

[ ] ArduPilot [X] PX4

Version: master

Diagnostics a sample of rostopic echo: header: seq: 10 stamp: secs: 1861 nsecs: 652000000 frame_id: "base_footprint" pose: position: latitude: 19.1345485 longitude: 72.9122298 altitude: -48.3160189477 orientation: x: 0.0 y: 0.0 z: 0.0 w: 0.0 Just curious, does frame_id field play any role? I tried with _localorigin too, with no effect.

Thanks.

burak-yildizoz commented 4 years ago

Make sure that you publish a point that is above the drone's altitude. It may seem a silly suggestion but it is possible that one can forget GPS altitude can be far above local altitude.

jungx148 commented 4 years ago

@swaroophangal I am also struggling to solve a similar problem with you by using a code given by @tropappar in the above. That is, by sending position a setpoint through "mavros/setpoint_position/global," the vehicle successfully enters into offboard and even arms but sits on the ground. I tried to send a position setpoint "mavros/setpoint_raw/global" as well, but also failed with a same phenomenon (i.e., the vehicle successfully enters into offboard and even arms but sits on the ground.)

@burak-yildizoz Also, I rechecked that the published point to be above the drone's GPS altitude (the code given by @tropappar in the above solved this by using "goal_position.altitude = goal_position.altitude + 5.0;"). In particular, I played around with the "+5" with other values but ended up with no success.

Could someone help? Thank you!

-- MAVROS version and platform

Mavros: master ROS: Kinetic Ubuntu: 16.04 Autopilot type and version

[ ] ArduPilot [X] PX4

Version: master

Diagnostics a sample of rostopic echo /mavros/global_position/global: header: seq: 206 stamp: secs: 1286 nsecs: 28000000 frame_id: "base_link" status: status: 0 service: 1 latitude: 47.3977421 longitude: 8.5455932 altitude: 535.327916774 position_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] position_covariance_type: 2

rqt_graph

Jaeyoung-Lim commented 4 years ago

@jungx148 You need to check if your fmu is receiving the mavlink setpoints you are sending.

jungx148 commented 4 years ago

@Jaeyoung-Lim Hello, thank you very much for all your efforts for the Pixhawk community and also the quick response.

I am having previously stated issues while I was working on converting your previously published "modudculab_ros" project (link) which was built for local offboard control to the one of global offboard control.

Regarding your comment, "You need to check if your fmu is receiving the mavlink setpoints you are sending," I checked a parameter, "POSITION_TARGET_GLOBAL_INT (link)," through QGC and noticed that no mavlink setpoints were indeed received by fmu as you stated as shown in the below picture captured from QGC screen.

QGC

In addition, I attached the output of roslaunch modudculab_ros ctrl_pos_gazebo_offb.launch in the below.

`sunghunjung2@gcs:~$ roslaunch modudculab_ros ctrl_pos_gazebo_offb.launch ... logging to /home/sunghunjung2/.ros/log/c7421548-ea08-11ea-8a18-b052161d0343/roslaunch-gcs-16794.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://gcs:35869/

SUMMARY

PARAMETERS

/mavros/conn_heartbeat: 5.0
/mavros/conn_timeout: 5.0
/mavros/fcu_url: udp://:14540@127....
/mavros/frame_id: world
/mavros/gcs_url:
/mavros/mocap/use_pose: False
/mavros/mocap/use_tf: True
/mavros/startup_px4_usb_quirk: True
/mavros/target_component_id: 1
/mavros/target_system_id: 1
/rosdistro: kinetic
/rosversion: 1.12.15

NODES / mavros (mavros/mavros_node) offb_node (modudculab_ros/pub_setpoints_traj)

ROS_MASTER_URI=http://localhost:11311

process[mavros-1]: started with pid [16811] process[offb_node-2]: started with pid [16812] [ INFO] [1598713932.574302223]: Waiting for FCU connection... [ INFO] [1598713932.630180762]: FCU URL: udp://:14540@127.0.0.1:14557 [ INFO] [1598713932.632217367]: udp0: Bind address: 0.0.0.0:14540 [ INFO] [1598713932.632330437]: udp0: Remote address: 127.0.0.1:14557 [ INFO] [1598713932.632484805]: GCS bridge disabled [ INFO] [1598713932.639316949]: udp0: Remote address: 127.0.0.1:14580 [ INFO] [1598713932.646535787]: Plugin 3dr_radio loaded [ INFO] [1598713932.647964566]: Plugin 3dr_radio initialized [ INFO] [1598713932.648032614]: Plugin actuator_control loaded [ INFO] [1598713932.655212872]: Plugin actuator_control initialized [ INFO] [1598713932.659214052]: Plugin adsb loaded [ INFO] [1598713932.663795271]: Plugin adsb initialized [ INFO] [1598713932.663884912]: Plugin altitude loaded [ INFO] [1598713932.665120898]: Plugin altitude initialized [ INFO] [1598713932.665205126]: Plugin cam_imu_sync loaded [ INFO] [1598713932.665857482]: Plugin cam_imu_sync initialized [ INFO] [1598713932.665962528]: Plugin command loaded [ INFO] [1598713932.676583003]: Plugin command initialized [ INFO] [1598713932.676686573]: Plugin companion_process_status loaded [ INFO] [1598713932.680451720]: Plugin companion_process_status initialized [ INFO] [1598713932.680538169]: Plugin debug_value loaded [ INFO] [1598713932.686312462]: Plugin debug_value initialized [ INFO] [1598713932.686466097]: Plugin distance_sensor loaded [ WARN] [1598713932.688396981]: DS: plugin not configured! [ INFO] [1598713932.688499978]: Plugin distance_sensor initialized [ INFO] [1598713932.688607213]: Plugin esc_status loaded [ INFO] [1598713932.690715936]: Plugin esc_status initialized [ INFO] [1598713932.690843038]: Plugin fake_gps loaded [ INFO] [1598713932.710054220]: Plugin fake_gps initialized [ INFO] [1598713932.710206748]: Plugin ftp loaded [ INFO] [1598713932.720469896]: Plugin ftp initialized [ INFO] [1598713932.720732392]: Plugin global_position loaded [ INFO] [1598713932.745942903]: Plugin global_position initialized [ INFO] [1598713932.746061593]: Plugin gps_rtk loaded [ INFO] [1598713932.751198913]: Plugin gps_rtk initialized [ INFO] [1598713932.751319925]: Plugin gps_status loaded [ INFO] [1598713932.756410332]: Plugin gps_status initialized [ INFO] [1598713932.756538879]: Plugin hil loaded [ INFO] [1598713932.777252602]: Plugin hil initialized [ INFO] [1598713932.777363722]: Plugin home_position loaded [ INFO] [1598713932.784108936]: Plugin home_position initialized [ INFO] [1598713932.784244221]: Plugin imu loaded [ INFO] [1598713932.796900612]: Plugin imu initialized [ INFO] [1598713932.797023919]: Plugin landing_target loaded [ INFO] [1598713932.812620166, 525.508000000]: Plugin landing_target initialized [ INFO] [1598713932.812769284, 525.508000000]: Plugin local_position loaded [ INFO] [1598713932.823191443, 525.516000000]: Plugin local_position initialized [ INFO] [1598713932.823332049, 525.516000000]: Plugin log_transfer loaded [ INFO] [1598713932.829462875, 525.524000000]: Plugin log_transfer initialized [ INFO] [1598713932.829621199, 525.524000000]: Plugin manual_control loaded [ INFO] [1598713932.834622492, 525.528000000]: Plugin manual_control initialized [ INFO] [1598713932.834747185, 525.532000000]: Plugin mocap_pose_estimate loaded [ INFO] [1598713932.841112669, 525.536000000]: Plugin mocap_pose_estimate initialized [ INFO] [1598713932.841216643, 525.536000000]: Plugin mount_control loaded [ INFO] [1598713932.847855265, 525.544000000]: Plugin mount_control initialized [ INFO] [1598713932.848004969, 525.544000000]: Plugin obstacle_distance loaded [ INFO] [1598713932.852958083, 525.548000000]: Plugin obstacle_distance initialized [ INFO] [1598713932.853087457, 525.548000000]: Plugin odom loaded [ INFO] [1598713932.863511077, 525.560000000]: Plugin odom initialized [ INFO] [1598713932.863643202, 525.560000000]: Plugin onboard_computer_status loaded [ INFO] [1598713932.867747275, 525.564000000]: Plugin onboard_computer_status initialized [ INFO] [1598713932.867893841, 525.564000000]: Plugin param loaded [ INFO] [1598713932.873643707, 525.568000000]: Plugin param initialized [ INFO] [1598713932.873750818, 525.568000000]: Plugin px4flow loaded [ INFO] [1598713932.889263497, 525.584000000]: Plugin px4flow initialized [ INFO] [1598713932.889393131, 525.584000000]: Plugin rangefinder loaded [ INFO] [1598713932.890654247, 525.588000000]: Plugin rangefinder initialized [ INFO] [1598713932.890809291, 525.588000000]: Plugin rc_io loaded [ INFO] [1598713932.901011153, 525.596000000]: Plugin rc_io initialized [ INFO] [1598713932.901200080, 525.596000000]: Plugin safety_area loaded [ INFO] [1598713932.910855240, 525.608000000]: Plugin safety_area initialized [ INFO] [1598713932.910990631, 525.608000000]: Plugin setpoint_accel loaded [ INFO] [1598713932.915734786, 525.612000000]: Plugin setpoint_accel initialized [ INFO] [1598713932.915940005, 525.612000000]: Plugin setpoint_attitude loaded [ INFO] [1598713932.933403072, 525.628000000]: Plugin setpoint_attitude initialized [ INFO] [1598713932.933551083, 525.628000000]: Plugin setpoint_position loaded [ INFO] [1598713932.961229635, 525.656000000]: Plugin setpoint_position initialized [ INFO] [1598713932.961423294, 525.656000000]: Plugin setpoint_raw loaded [ INFO] [1598713932.975968050, 525.672000000]: Plugin setpoint_raw initialized [ INFO] [1598713932.976119496, 525.672000000]: Plugin setpoint_trajectory loaded [ INFO] [1598713932.983982461, 525.680000000]: Plugin setpoint_trajectory initialized [ INFO] [1598713932.984193434, 525.680000000]: Plugin setpoint_velocity loaded [ INFO] [1598713932.997254621, 525.692000000]: Plugin setpoint_velocity initialized [ INFO] [1598713932.997463609, 525.692000000]: Plugin sys_status loaded [ INFO] [1598713933.014681777, 525.708000000]: Plugin sys_status initialized [ INFO] [1598713933.014873290, 525.708000000]: Plugin sys_time loaded [ INFO] [1598713933.030238527, 525.724000000]: TM: Timesync mode: MAVLINK [ INFO] [1598713933.033139696, 525.728000000]: Plugin sys_time initialized [ INFO] [1598713933.033276975, 525.728000000]: Plugin trajectory loaded [ INFO] [1598713933.044112944, 525.740000000]: Plugin trajectory initialized [ INFO] [1598713933.044248744, 525.740000000]: Plugin vfr_hud loaded [ INFO] [1598713933.045033751, 525.740000000]: Plugin vfr_hud initialized [ INFO] [1598713933.045161699, 525.740000000]: Plugin vibration loaded [ INFO] [1598713933.046751733, 525.740000000]: Plugin vibration initialized [ INFO] [1598713933.046936799, 525.740000000]: Plugin vision_pose_estimate loaded [ INFO] [1598713933.074073664, 525.764000000]: Plugin vision_pose_estimate initialized [ INFO] [1598713933.074199255, 525.764000000]: Plugin vision_speed_estimate loaded [ INFO] [1598713933.079574881, 525.772000000]: Plugin vision_speed_estimate initialized [ INFO] [1598713933.079713935, 525.772000000]: Plugin waypoint loaded [ INFO] [1598713933.090535112, 525.780000000]: Plugin waypoint initialized [ INFO] [1598713933.090690579, 525.780000000]: Plugin wheel_odometry loaded [ WARN] [1598713933.109775730, 525.800000000]: WO: Not all wheels have parameters specified (0/2). [ WARN] [1598713933.109826161, 525.800000000]: WO: No odometry computations will be performed. [ INFO] [1598713933.109857026, 525.800000000]: Plugin wheel_odometry initialized [ INFO] [1598713933.110067122, 525.800000000]: Plugin wind_estimation loaded [ INFO] [1598713933.111000388, 525.800000000]: Plugin wind_estimation initialized [ INFO] [1598713933.111078871, 525.800000000]: Autostarting mavlink via USB on PX4 [ INFO] [1598713933.111183424, 525.800000000]: Built-in SIMD instructions: SSE, SSE2 [ INFO] [1598713933.111212642, 525.800000000]: Built-in MAVLink package version: 2020.8.8 [ INFO] [1598713933.111241943, 525.800000000]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad icarous matrixpilot paparazzi slugs standard uAvionix ualberta [ INFO] [1598713933.111266530, 525.800000000]: MAVROS started. MY ID 1.240, TARGET ID 1.1 [ INFO] [1598713933.125343374, 525.812000000]: IMU: High resolution IMU detected! [ INFO] [1598713933.128846623, 525.820000000]: IMU: Attitude quaternion IMU detected! [ INFO] [1598713933.208070983, 525.900000000]: Got global position: [47.40, 8.55, 535.42] [ INFO] [1598713933.842328830, 526.536000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot [ INFO] [1598713933.844470149, 526.536000000]: IMU: High resolution IMU detected! [ INFO] [1598713933.846264107, 526.540000000]: IMU: Attitude quaternion IMU detected! [ INFO] [1598713934.006045975, 526.700000000]: FCU connected [ INFO] [1598713934.006167891, 526.700000000]: GPS position received [ INFO] [1598713934.849291372, 527.540000000]: WP: Using MISSION_ITEM_INT [ INFO] [1598713934.849435224, 527.540000000]: VER: 1.1: Capabilities 0x000000000000e4ef [ INFO] [1598713934.849538235, 527.540000000]: VER: 1.1: Flight software: 010b00c0 (a18f22bd91000000) [ INFO] [1598713934.849628125, 527.540000000]: VER: 1.1: Middleware software: 010b00c0 (a18f22bd91000000) [ INFO] [1598713934.849751519, 527.540000000]: VER: 1.1: OS software: 040f00ff (0000000000000000) [ INFO] [1598713934.849901086, 527.540000000]: VER: 1.1: Board hardware: 00000001 [ INFO] [1598713934.850116229, 527.540000000]: VER: 1.1: VID/PID: 0000:0000 [ INFO] [1598713934.850322861, 527.540000000]: VER: 1.1: UID: 4954414c44494e4f [ WARN] [1598713934.851805875, 527.540000000]: CMD: Unexpected command 520, result 0 [ INFO] [1598713936.016282214, 528.700000000]: OFFBOARD enabled [ INFO] [1598713936.033795090, 528.720000000]: Vehicle armed [ INFO] [1598713936.033912868, 528.720000000]: At altitude 535.41 [ INFO] [1598713937.116983445, 529.800000000]: At altitude 535.42 [ INFO] [1598713938.128741116, 530.800000000]: At altitude 535.41 [ INFO] [1598713939.133153162, 531.800000000]: At altitude 535.41 [ INFO] [1598713940.135142792, 532.800000000]: At altitude 535.39 [ INFO] [1598713941.238954898, 533.800000000]: At altitude 535.36 [ INFO] [1598713942.241542686, 534.800000000]: At altitude 535.35 [ INFO] [1598713943.246164986, 535.800000000]: At altitude 535.35 [ INFO] [1598713944.245777661, 536.800000000]: At altitude 535.33 [ INFO] [1598713945.247236209, 537.800000000]: At altitude 535.31 [ INFO] [1598713946.248834592, 538.800000000]: At altitude 535.30 [ INFO] [1598713947.351206878, 539.800000000]: At altitude 535.28 [ INFO] [1598713948.352784079, 540.800000000]: At altitude 535.27 [ INFO] [1598713949.092416093, 541.536000000]: WP: mission received [ INFO] [1598713949.361873775, 541.800000000]: At altitude 535.26 [ INFO] [1598713950.362903668, 542.800000000]: At altitude 535.25 [ INFO] [1598713951.369719013, 543.800000000]: At altitude 535.25 [ INFO] [1598713952.374589144, 544.800000000]: At altitude 535.25 [ INFO] [1598713953.373353193, 545.800000000]: At altitude 535.24 [ INFO] [1598713954.380141663, 546.800000000]: At altitude 535.24 [ INFO] [1598713955.378495116, 547.800000000]: At altitude 535.24 [ INFO] [1598713956.378876657, 548.800000000]: At altitude 535.24 [ INFO] [1598713957.386021610, 549.800000000]: At altitude 535.25 [ INFO] [1598713958.391286494, 550.800000000]: At altitude 535.25 [ INFO] [1598713959.494104279, 551.800000000]: At altitude 535.25 [ INFO] [1598713960.503301681, 552.800000000]: At altitude 535.26 [ INFO] [1598713961.618806685, 553.800000000]: At altitude 535.27 [ INFO] [1598713962.619243815, 554.800000000]: At altitude 535.26 `

-- Regarding this issue, I suspect the following two parts.

SUSPECT 1: I suspect this problem is related to the installed MAVROS version on my desktop. I installed ros-kinetic-mavros* using apt command and also saved the latest mavros git package in the catkin_ws/src folder.

SUSPECT 2: When I used

ros::Publisher goal_pos_pub = nh.advertise < mavros_msgs::GlobalPositionTarget > ("mavros/setpoint_position/global", 10);,

I received the following error message

[ERROR] [1598713832.833795288, 426.168000000]: Client [/mavros] wants topic /mavros/setpoint_position/global to have datatype/md5sum [geographic_msgs/GeoPoseStamped/cc409c8ed6064d8a846fa207bf3fba6b], but our version has [mavros_msgs/GlobalPositionTarget/076ded0190b9e045f9c55264659ef102]. Dropping connection.

And then, I replaced it with

ros::Publisher goal_pos_pub = nh.advertise < mavros_msgs::GlobalPositionTarget > ("geographic_msgs/GeoPoseStamped", 10);

and it did not result in any error message. However, in either way, the parameter, "POSITION_TARGET_GLOBAL_INT" does not appear in the QGC mavlink inspector.

Again, thank you very much for your invaluable time to review this issue.

jungx148 commented 4 years ago

I couldn't solve the above problem, and so I change to use mavros_msgs to pull waypoints from QGC, insert externally calculated yaw angle, and push combined waypoints (pulled waypoints from QGC with externally calculated yaw angle).

In this way, I could successfully control MAV as I wanted. Thank you very much.

Jaeyoung-Lim commented 4 years ago

@jungx148 That is not the solution and just a workaround.

Have you checked if the setpoints you are sending is reaching the fmu?

burak-yildizoz commented 4 years ago

@jungx148 Is it possible that your problem is related to #1414?

mariofaragalla commented 3 years ago

Did any anyone reach any conclusion yet??

I tried publishing on these 3 topics but with no luck

"/mavros/setpoint_raw/global" "/mavros/setpoint_raw/target_global" "/mavros/setpoint_position/global"

N.B: I doing a gazebo simulation only

Well I recently found this error: [ERROR] [1613491606.419150067, 3031.685000000]: Client [/drone2/mavros] wants topic /drone2/mavros/setpoint_position/global to have datatype/md5sum [geographic_msgs/GeoPoseStamped/cc409c8ed6064d8a846fa207bf3fba6b], but our version has [mavros_msgs/GlobalPositionTarget/076ded0190b9e045f9c55264659ef102]. Dropping connection.

mariofaragalla commented 3 years ago

Well the topic /mavros/setpoint_position/global finally worked (but with altitude offset?). From 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.

abhiramdapke commented 3 years ago

I am also trying to do the same maneuver i.e. publish mavros_msgs::GlobalPositionTarget message on the mavros/setpoint_raw/global topic and it works fine. The drone takes off and goes to the desired waypoint. I wanted to ask if anyone has successfully navigated the drone to multiple waypoints and if we need a logic to check if the waypoint is reached? The reason I am asking this is that I am not sure how the gazebo sets its home position, so when the drone goes to any random lat lon alt provided, it goes to that waypoint indefinitely. Is there any way to check if that waypoint is reached and then provide another waypoint? Any suggestions would be highly appreciated. Thanks in advance.

burak-yildizoz commented 3 years ago

I am also trying to do the same maneuver i.e. publish mavros_msgs::GlobalPositionTarget message on the mavros/setpoint_raw/global topic and it works fine. The drone takes off and goes to the desired waypoint. I wanted to ask if anyone has successfully navigated the drone to multiple waypoints and if we need a logic to check if the waypoint is reached? The reason I am asking this is that I am not sure how the gazebo sets its home position, so when the drone goes to any random lat lon alt provided, it goes to that waypoint indefinitely. Is there any way to check if that waypoint is reached and then provide another waypoint? Any suggestions would be highly appreciated. Thanks in advance.

Just keep publishing global position setpoints until the drone is inside an acceptable region, like ~50 cm in xy-plane and 10 cm in z axis. Then, publish another point.

abhiramdapke commented 3 years ago

Didn't quite follow. Could you please send a simple sample code so that I can get an idea? I tried a couple of things but those didn't work. Z-axis 10cm or m? Thanks.

burak-yildizoz commented 3 years ago

Didn't quite follow. Could you please send a simple sample code so that I can get an idea? I tried a couple of things but those didn't work. Z-axis 10cm or m? Thanks.

I wrote it just to give you the idea. Subscribe to global position, either convert to cartesian coordinates or just simply have a threshold in lat/lon degrees around 1e-6 such that the difference between the drone and the desired point is lower than a threshold. You need to treat altitude differently, because its value is not in degrees but in meters. If the drone is inside a box, waypoint reached.

abhiramdapke commented 3 years ago

Currently, I am using sensor_msgs::NavSatFix to get the position of the drone. When you say, subscribe to global position, are you talking about this subscriber? Also, it would be really helpful if you could elaborate a little bit on how to create a threshold in lat/lon degrees around 0.000001 and how to check if the drone is inside the box? If you have already implemented it, could you please share a small snippet of the logic that you are mentioning? That would make things a lot easier. Thanks a lot.

abhiramdapke commented 3 years ago

Hello everyone, If anyone has solved the navigation of a drone with multiple waypoints in the global frame, could you please take a look at [https://github.com/mavlink/mavros/issues/1553} and help me with the issue. I have been trying to implement it for a few weeks but not able to get to a solution. I just want to navigate the drone using lat lon alt to multiple waypoints. Thanks in advance.

Wuxinxiaoshifu commented 3 years ago

I have to translate from global to local and then send in mavros/setpoint_position/local otherwise it won't work. I am curious, is it me the only one with this problem? Is someone able to reproduce the aircraft offboard example just by publishing in mavros/setpoint_position/global?

s

include <ros/ros.h>

include <mavros_msgs/CommandBool.h>

include <mavros_msgs/SetMode.h>

include <mavros_msgs/State.h>

include <mavros_msgs/GlobalPositionTarget.h>

include <sensor_msgs/NavSatFix.h>

// global variables mavros_msgs::State current_state; sensor_msgs::NavSatFix global_position; bool global_position_received = false;

// callback functions void globalPosition_cb(const sensor_msgs::NavSatFix::ConstPtr& msg) { global_position = *msg; global_position_received = true; ROS_INFO_ONCE("Got global position: [%.2f, %.2f, %.2f]", msg->latitude, msg->longitude, msg->altitude); }

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

// main function int main(int argc, char **argv) { ros::init(argc, argv, "test"); ros::NodeHandle nh;

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");
ros::Subscriber state_sub = nh.subscribe < mavros_msgs::State > ("mavros/state", 10, state_cb);
ros::Subscriber global_pos_sub = nh.subscribe < sensor_msgs::NavSatFix > ("mavros/global_position/global", 1, globalPosition_cb);
ros::Publisher goal_pos_pub = nh.advertise < mavros_msgs::GlobalPositionTarget > ("mavros/setpoint_position/global", 10);

ros::Rate rate(10);

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

// wait for position information
while (ros::ok() && !global_position_received) {
    ROS_INFO_ONCE("Waiting for GPS signal...");
    ros::spinOnce();
    rate.sleep();
}
ROS_INFO("GPS position received");

// set target position
mavros_msgs::GlobalPositionTarget goal_position;
goal_position.latitude = global_position.latitude;
goal_position.longitude = global_position.longitude;
goal_position.altitude = global_position.altitude;

// send a few setpoints before starting
for (int i=0; i<20; ++i) {
    goal_position.header.stamp = ros::Time::now();
    goal_pos_pub.publish(goal_position);
    ros::spinOnce();
    rate.sleep();
}

// set mode
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.base_mode = 0;
offb_set_mode.request.custom_mode = "OFFBOARD";
if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
    ROS_INFO("OFFBOARD enabled");
} else {
    ROS_ERROR("Failed to set OFFBOARD");
}

// arm
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
    ROS_INFO("Vehicle armed");
} else {
    ROS_ERROR("Arming failed");
}

// take off to 5m above ground
goal_position.altitude = goal_position.altitude + 5.0;
while (ros::ok()) {
    goal_position.header.stamp = ros::Time::now();
    goal_pos_pub.publish(goal_position);
    ros::spinOnce();
    ROS_INFO_THROTTLE(1, "At altitude %.2f", global_position.altitude);
    rate.sleep();
}

return 0;

}

did you had this problem?

Client [/mavros] wants topic /mavros/setpoint_position/global to have datatype/md5sum [geographic_msgs/GeoPoseStamped/cc409c8ed6064d8a846fa207bf3fba6b], but our version has [mavros_msgs/GlobalPositionTarget/076ded0190b9e045f9c55264659ef102]. Dropping connection.

burak-yildizoz commented 3 years ago

@Wuxinxiaoshifu You should probably run a sudo apt upgrade to get the newest version of ros-{distro}-mavros and other packages if you installed them via apt, then rebuild your workspace. You may need to update PX4 as well (git checkout, make). If the problem persists and you can't find a similar issue, you should create a new one. I remember facing this problem, so even if there is not a similar issue, the problem and your solution steps would be beneficial.

leomichalski commented 2 years ago

I adapted @tropappar's example to work with geographic_msgs/GeoPoseStamped. Just a warning: there's something weird with the example and/or with my Gazebo's GPS. My vehicle altitude is 0m, but the altitude I get from the GPS is 43.39m. Then the vehicle takes off to almost 2 times the altitude read from the GPS (91.60m +- 0.1m).

Here's the adapted example:

#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <ros/ros.h>
#include <geographic_msgs/GeoPoseStamped.h>
// #include <mavros_msgs/GlobalPositionTarget.h>
#include <sensor_msgs/NavSatFix.h>

// global variables
mavros_msgs::State current_state;
sensor_msgs::NavSatFix global_position;
bool global_position_received = false;

// callback functions
void globalPosition_cb(const sensor_msgs::NavSatFix::ConstPtr& msg) {
    global_position = *msg;
    global_position_received = true;
    ROS_INFO_ONCE("Got global position: [%.2f, %.2f, %.2f]", msg->latitude, msg->longitude, msg->altitude);
}

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

// main function
int main(int argc, char **argv) {
    ros::init(argc, argv, "test");
    ros::NodeHandle nh;

    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");
    ros::Subscriber state_sub = nh.subscribe < mavros_msgs::State > ("mavros/state", 10, state_cb);
    ros::Subscriber global_pos_sub = nh.subscribe < sensor_msgs::NavSatFix > ("mavros/global_position/global", 1, globalPosition_cb);
    ros::Publisher goal_pos_pub = nh.advertise < geographic_msgs::GeoPoseStamped > ("mavros/setpoint_position/global", 10);
    // ros::Publisher goal_pos_pub = nh.advertise < mavros_msgs::GlobalPositionTarget > ("mavros/setpoint_position/global", 10);

    ros::Rate rate(10);

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

    // wait for position information
    while (ros::ok() && !global_position_received) {
        ROS_INFO_ONCE("Waiting for GPS signal...");
        ros::spinOnce();
        rate.sleep();
    }
    ROS_INFO("GPS position received");

    // set target position
    geographic_msgs::GeoPoseStamped goal_position;
    goal_position.header.stamp = ros::Time::now();
    goal_position.pose.position.latitude = global_position.latitude;
    goal_position.pose.position.longitude = global_position.longitude;
    goal_position.pose.position.altitude = global_position.altitude;

    // send a few setpoints before starting
    for (int i=0; i<20; ++i) {
        goal_position.header.stamp = ros::Time::now();
        goal_pos_pub.publish(goal_position);
        ros::spinOnce();
        rate.sleep();
    }

    // set mode
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.base_mode = 0;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
        ROS_INFO("OFFBOARD enabled");
    } else {
        ROS_ERROR("Failed to set OFFBOARD");
    }

    // arm
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
        ROS_INFO("Vehicle armed");
    } else {
        ROS_ERROR("Arming failed");
    }

    // take off to 5m above ground
    goal_position.pose.position.altitude = goal_position.pose.position.altitude + 5.0;
    while (ros::ok()) {
        goal_position.header.stamp = ros::Time::now();
        goal_pos_pub.publish(goal_position);
        ros::spinOnce();
        ROS_INFO_THROTTLE(1, "At altitude %.2f", global_position.altitude);
        rate.sleep();
    }

    return 0;
}

leomichalski commented 2 years ago

I fixed the weird thing about the altitude. It was related to this common pitfall: https://wiki.ros.org/mavros#mavros.2FPlugins.Avoiding_Pitfalls_Related_to_Ellipsoid_Height_and_Height_Above_Mean_Sea_Level

#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <ros/ros.h>
#include <geographic_msgs/GeoPoseStamped.h>
// #include <mavros_msgs/GlobalPositionTarget.h>
#include <sensor_msgs/NavSatFix.h>

#include "GeographicLib/Geoid.hpp"

// global variables
mavros_msgs::State current_state;
// sensor_msgs::NavSatFix global_position;
geographic_msgs::GeoPoseStamped global_position;
bool global_position_received = false;
GeographicLib::Geoid _egm96("egm96-5");  // WARNING: not thread safe

double calc_geoid_height(double lat, double lon) {
    return _egm96(lat, lon);
}
double amsl_to_ellipsoid_height(double lat, double lon, double amsl) {
  return amsl + GeographicLib::Geoid::GEOIDTOELLIPSOID * calc_geoid_height(lat, lon);
}
double ellipsoid_height_to_amsl(double lat, double lon, double ellipsoid_height) {
  return ellipsoid_height + GeographicLib::Geoid::ELLIPSOIDTOGEOID * calc_geoid_height(lat, lon);
}

// callback functions
void globalPosition_cb(const sensor_msgs::NavSatFix::ConstPtr& msg) {
    global_position.pose.position.altitude = ellipsoid_height_to_amsl(msg->latitude, msg->longitude, msg->altitude);
    global_position.header.stamp = msg->header.stamp;
    global_position.pose.position.latitude = msg->latitude;
    global_position.pose.position.longitude = msg->longitude;
    global_position_received = true;
    ROS_INFO_ONCE("Got global position: [%.2f, %.2f, %.2f]", global_position.pose.position.latitude, global_position.pose.position.longitude, global_position.pose.position.altitude);
}

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

// main function
int main(int argc, char **argv) {
    ros::init(argc, argv, "test");
    ros::NodeHandle nh;

    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");
    ros::Subscriber state_sub = nh.subscribe < mavros_msgs::State > ("mavros/state", 10, state_cb);
    ros::Subscriber global_pos_sub = nh.subscribe < sensor_msgs::NavSatFix > ("mavros/global_position/global", 1, globalPosition_cb);
    ros::Publisher goal_pos_pub = nh.advertise < geographic_msgs::GeoPoseStamped > ("mavros/setpoint_position/global", 10);
    // ros::Publisher goal_pos_pub = nh.advertise < mavros_msgs::GlobalPositionTarget > ("mavros/setpoint_position/global", 10);

    ros::Rate rate(10);

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

    // wait for position information
    while (ros::ok() && !global_position_received) {
        ROS_INFO_ONCE("Waiting for GPS signal...");
        ros::spinOnce();
        rate.sleep();
    }
    ROS_INFO("GPS position received");

    // set target position
    geographic_msgs::GeoPoseStamped goal_position;
    goal_position.header.stamp = ros::Time::now();
    goal_position.pose.position.latitude = global_position.pose.position.latitude;
    goal_position.pose.position.longitude = global_position.pose.position.longitude;
    goal_position.pose.position.altitude = global_position.pose.position.altitude;

    // send a few setpoints before starting
    for (int i=0; i<20; ++i) {
        goal_position.header.stamp = ros::Time::now();
        goal_pos_pub.publish(goal_position);
        ros::spinOnce();
        rate.sleep();
    }

    // set mode
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.base_mode = 0;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
        ROS_INFO("OFFBOARD enabled");
    } else {
        ROS_ERROR("Failed to set OFFBOARD");
    }

    // arm
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
        ROS_INFO("Vehicle armed");
    } else {
        ROS_ERROR("Arming failed");
    }

    // take off to 5m above ground
    goal_position.pose.position.altitude = goal_position.pose.position.altitude + 5.0;
    while (ros::ok()) {
        goal_position.header.stamp = ros::Time::now();
        goal_pos_pub.publish(goal_position);
        ros::spinOnce();
        ROS_INFO_THROTTLE(1, "At altitude %.2f", global_position.pose.position.altitude);
        rate.sleep();
    }

    return 0;
}

mavlink / mavros

Offboard control example setpoint_position/global not arming #903

Issue details

MAVROS version and platform

Autopilot type and version

Node logs

Diagnostics

Check ID

SUMMARY

include <ros/ros.h>

include <mavros_msgs/CommandBool.h>

include <mavros_msgs/SetMode.h>

include <mavros_msgs/State.h>

include <mavros_msgs/GlobalPositionTarget.h>

include <sensor_msgs/NavSatFix.h>