mavros
mavros copied to clipboard
mavlink
Multiple issues navigating to waypoints using mavros_msgs::globalpositiontarget
Mavros: 1.5 ROS: Melodic Ubuntu: 18.04
[ Y ] ArduPilot
Hi guys, I am trying to implement a GPS function by passing multiple waypoints to the drone using mavros and would like to implement it with a real drone with a companion computer. Currently, I am trying to accomplish it in the simulation space(Gazebo). I am using mavros_msgs::globalpositiontarget msg to pass the waypoints and sensor_msgs::NatSatFix to get the location of the drone.
The subscriber callback for getting the global position is -
void globalPosition_cb(const sensor_msgs::NavSatFix::ConstPtr& msg) {
global_position = *msg;
gps_position_received = true;
ROS_INFO("Received global position: [%f, %f, %f]", global_position.latitude, global_position.longitude, global_position.altitude);
}
For this, the subscriber is -
ros::Subscriber global_pos_sub = waypoint_node.subscribe<sensor_msgs::NavSatFix>("mavros/global_position/global", 10, globalPosition_cb);
The function for the waypoint is -
void set_waypoint(float lat, float lon, float alt)
{
mavros_msgs::GlobalPositionTarget goal_position;
goal_position.coordinate_frame = goal_position.FRAME_GLOBAL_TERRAIN_ALT;
goal_position.type_mask = goal_position.IGNORE_VX | goal_position.IGNORE_VY | goal_position.IGNORE_VZ | goal_position.IGNORE_AFX | goal_position.IGNORE_AFY | goal_position.IGNORE_AFZ | goal_position.IGNORE_YAW | goal_position.IGNORE_YAW_RATE;
goal_position.latitude = lat;
goal_position.longitude = lon;
goal_position.altitude = alt;
global_pos_pub_gps.publish(goal_position);
}
For this, the Publisher is -
ros::Publisher global_pos_pub_gps = waypoint_node.advertise<mavros_msgs::GlobalPositionTarget>("/mavros/setpoint_raw/global",10);
A snippet of the script that I am using is like this -
struct waypoint
{
float lat;
float lon;
float alt;
};
ros::Rate rate(2.0);
wait4connect();
wait4start();
takeoff(5);
sleep(4);
std::vector<waypoint> waypointlist;
waypoint nextwp;
nextwp.lat = 0;
nextwp.lon = 0;
nextwp.alt = 5;
waypointlist.push_back(nextwp);
nextwp.lat = -35.3613525;
nextwp.lon = 149.1579742;
nextwp.alt = 5;
waypointlist.push_back(nextwp);
nextwp.lat = -35.3613530;
nextwp.lon = 149.1579735;
nextwp.alt = 5;
waypointlist.push_back(nextwp);
nextwp.lat = -35.3613535;
nextwp.lon = 149.1579730;
nextwp.alt = 5;
waypointlist.push_back(nextwp);
int i = 0;
//wait for position information
while(ros::ok() && !gps_position_received)
{
ROS_INFO_ONCE("Waiting for GPS signal");
ros::spinOnce();
rate.sleep();
ROS_INFO_ONCE("GPS position received");
if(i < waypointlist.size())
{
set_waypoint(waypointlist[i].lat,waypointlist[i].lon,waypointlist[i].alt);
ROS_INFO("GPS position sent: [%f %f %f]", waypointlist[i].lat, waypointlist[i].lon, waypointlist[i].alt);
i++;
sleep(4);
/*while (waypoint_reached(waypointlist[i].lat, waypointlist[i].lon)!= 1)
{
if (waypoint_reached(waypointlist[i].lat, waypointlist[i].lon)== 1)
{
i++;
break;
}
else
{
continue;
}}*/
//sleep(5);
}
else
{
land();
}
}
return 0;
}
For checking if the waypoint is reached, I am using Haversine's formula to get the distance between two GPS waypoints like this. It is taking the target location and subtracting that from the current location(which is derived from the global_Position_cb) -
bool waypoint_reached(float tlat, float tlon)
{
double pi = 3.14159265359;
double degToRad = pi/180.0;
//Haversine Formula - Calculate great-circle distance betweeen two points
float currentLat = global_position.latitude;
float currentLong = global_position.longitude;
double eLat = degToRad*(tlat - currentLat);
double eLong = degToRad*(tlon - currentLong);
double a = sin(eLat/2)*sin(eLat/2) + cos(degToRad*currentLat)*cos(degToRad*tlat)*sin(eLong/2)*sin(eLong/2);
double c = 2*atan2(sqrt(a),sqrt(1-a));
float dist = (float) (6371000 * c);
double tolerance = 0.3;
if (abs(dist) < tolerance)
{
ROS_INFO_ONCE("Provided waypoint reached");
return true;
}
else
{
return false;
}
}
There are a multitude of issues I am facing -
-
when I run the above script, the drone takes-off at the provided altitude but before reaching that altitude, it starts navigating towards the waypoint provided by set_waypoint. so, it doesn't actually reach the given altitude i.e. the function doesn't get completed thoroughly but the program moves on to the next line. I am using a temporary sleep() so that it reaches the provided altitude but need a more concrete solution.
-
The same is happening with the set_waypoint function. On the terminal, I get ROS_INFO("GPS position sent: [%f %f %f]", waypointlist[i].lat, waypointlist[i].lon, waypointlist[i].alt) msg for all the 4 waypoints almost immediately so what happens is that the drone doesn't reach the provided waypoint and turns around towards the next one. Set_waypoint is not getting completed and the program is moving to the next line. I surmised this might be because of the while(rosok()) loop and the rate set for receiving msgs but couldn't figure out a workaround. That is why I wrote a waypoint_reached function to check if the waypoint is actually reached before the program moves ahead. Not sure if anything is wrong with the logic of the set_waypoint or the waypoint_reached function or implementation of both of them. When running this program with the waypoint_reached function uncommented, I only get GPS position sent: once and nothing happens after that as the drone is flying off to the waypoint.
-
Echoing the topic /mavros/global_position/global fluctuates the lat lon values and those values are not necessarily within the waypoints specified. I suppose the home position in gazebo is lat = -35.362 and lon = 149.161 because that's what I get when I echo this topic. Not sure how the gazebo sets its home position and whether it is possible to visualize the drone actually moving to multiple waypoints in the gazebo in WGS84. The reason I say this is because no matter what waypoint is provided to the set_waypoint, it goes in the direction of the waypoint indefinitely. I am not sure if the drone will stop after reaching that waypoint or just crash.
-
Echoing the topic /mavros/global_position/global shows an altitude of 608m but the drone rises to only 5m with the takeoff client. I found an issue on Github that mentions that the altitude is either in ASML or WGS84 which might cause an issue but didn't find a solution to fix it so that I can visualize the correct altitude in the gazebo during the simulation.
-
In the rqt graph, I am not sure is everything is connected where it needs to be. If you could confirm that, that would be great. Also, what is /rostopic_10114_161547 and /rostopic_10144_161547?
Log of rostopic echo /mavros/global_position/global
status:
status: 0
service: 1
latitude: -35.3624209
longitude: 149.161684
altitude: 608.365077318
position_covariance: [0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001]
position_covariance_type: 2
---
header:
seq: 307
stamp:
secs: 163
nsecs: 253040248
frame_id: "base_link"
status:
status: 0
service: 1
latitude: -35.362418
longitude: 149.1616713
altitude: 608.365064351
position_covariance: [0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001]
position_covariance_type: 2
---
header:
seq: 308
stamp:
secs: 163
nsecs: 493059935
frame_id: "base_link"
status:
status: 0
service: 1
latitude: -35.362415
longitude: 149.1616586
altitude: 608.365051502
position_covariance: [0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001]
position_covariance_type: 2
---
header:
seq: 309
stamp:
secs: 163
nsecs: 733092246
frame_id: "base_link"
status:
status: 0
service: 1
latitude: -35.3624121
longitude: 149.1616459
altitude: 608.355038533
position_covariance: [0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001]
position_covariance_type: 2
---
header:
seq: 310
stamp:
secs: 163
nsecs: 973113142
frame_id: "base_link"
status:
status: 0
service: 1
latitude: -35.3624091
longitude: 149.1616331
altitude: 608.355025552
position_covariance: [0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001]
position_covariance_type: 2
---
header:
seq: 311
stamp:
secs: 164
nsecs: 213140527
frame_id: "base_link"
status:
status: 0
service: 1
latitude: -35.3624062
longitude: 149.1616204
altitude: 608.355012582
position_covariance: [0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001]
position_covariance_type: 2
---
header:
seq: 312
stamp:
secs: 164
nsecs: 453161142
frame_id: "base_link"
status:
status: 0
service: 1
latitude: -35.3624032
longitude: 149.1616077
altitude: 608.354999729
position_covariance: [0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001, 0.0, 0.0, 0.0, 0.04000000000000001]
position_covariance_type: 2
---
Log of rostopic echo /mavros/setpoint_raw/global
coordinate_frame: 11
type_mask: 3576
latitude: 0.0
longitude: 0.0
altitude: 5.0
velocity:
x: 0.0
y: 0.0
z: 0.0
acceleration_or_force:
x: 0.0
y: 0.0
z: 0.0
yaw: 0.0
yaw_rate: 0.0
---
header:
seq: 34
stamp:
secs: 0
nsecs: 0
frame_id: ''
coordinate_frame: 11
type_mask: 3576
latitude: 0.0
longitude: 0.0
altitude: 5.0
velocity:
x: 0.0
y: 0.0
z: 0.0
acceleration_or_force:
x: 0.0
y: 0.0
z: 0.0
yaw: 0.0
yaw_rate: 0.0
---
header:
seq: 35
stamp:
secs: 0
nsecs: 0
frame_id: ''
coordinate_frame: 11
type_mask: 3576
latitude: 0.0
longitude: 0.0
altitude: 5.0
velocity:
x: 0.0
y: 0.0
z: 0.0
acceleration_or_force:
x: 0.0
y: 0.0
z: 0.0
yaw: 0.0
yaw_rate: 0.0
---
header:
seq: 36
stamp:
secs: 0
nsecs: 0
frame_id: ''
coordinate_frame: 11
type_mask: 3576
latitude: 0.0
longitude: 0.0
altitude: 5.0
velocity:
x: 0.0
y: 0.0
z: 0.0
acceleration_or_force:
x: 0.0
y: 0.0
z: 0.0
yaw: 0.0
yaw_rate: 0.0
---
header:
seq: 37
stamp:
secs: 0
nsecs: 0
frame_id: ''
coordinate_frame: 11
type_mask: 3576
latitude: 0.0
longitude: 0.0
altitude: 5.0
velocity:
x: 0.0
y: 0.0
z: 0.0
acceleration_or_force:
x: 0.0
y: 0.0
z: 0.0
yaw: 0.0
yaw_rate: 0.0
---
header:
seq: 38
stamp:
secs: 0
nsecs: 0
frame_id: ''
coordinate_frame: 11
type_mask: 3576
latitude: 0.0
longitude: 0.0
altitude: 5.0
velocity:
x: 0.0
y: 0.0
z: 0.0
acceleration_or_force:
x: 0.0
y: 0.0
z: 0.0
yaw: 0.0
yaw_rate: 0.0
---
header:
seq: 39
stamp:
secs: 0
nsecs: 0
frame_id: ''
coordinate_frame: 11
type_mask: 3576
latitude: 0.0
longitude: 0.0
altitude: 5.0
velocity:
x: 0.0
y: 0.0
z: 0.0
acceleration_or_force:
x: 0.0
y: 0.0
z: 0.0
yaw: 0.0
yaw_rate: 0.0
---
Image of rqt graph
I am working on these for the past few weeks and not yet able to accomplish a working GPS function so any insights or guidance would be highly appreciated. Please feel free to share a snippet of your code as that becomes easier to understand. Thanks in advance!
Try using mavros_msgs::Altitude::amsl as home altitude.
double latitude, longitude;
bool gps_received = false;
void get_global_pose (const sensor_msgs::NavSatFix::ConstPtr& msg) {
latitude = msg->latitude;
longitude = msg->longitude;
gps_received = true;
}
double altitude;
void get_altitude(const mavros_msgs::Altitude::ConstPtr &msg)
{
altitude = msg->amsl;
}
Set home_alt to altitude when gps_received becomes true, and publish to setpoint_position/global using geographic_msgs::GeoPoseStamped.
I've also been trying to control the drone with the same type of message, but with python. I've noticed issues too. Here is what I do:
- I takeoff by putting the drone in takeoff mode (
AUTO.TAKEOFF). - I look at the
mavros/extended_stateandmavros/global_position/rel_altmessages. I don't move on untillanded_state == 2and I've reached a high enough relative altitude (in my case I need to be at least 2 meters up). - I send 20 setpoint messages at a rate of 10 per second. Then I switch to offboard mode. I continue to send set point messages at the same rate after that.
- I look at
mavros/global_position/globalmessages. I check to see how many meters away my target position is from my current position. And this is where I get stuck.
The drone flies to the right latitude and longitude, but it's altitude is wrong. Here are the details from my log message:
distance: 47.21 meters
LATITUDE LONGITUDE ALTITUDE
current position: 47.3978429, 8.545869, 597.5515792976987
target position: 47.39784293752265, 8.545869057172045, 550.3417375283871
The altitude accounts for all the distance.
@burak-yildizoz I've also tried sending GeoPoseStamped messages and I get the same problem where the drone flies 47.2 meters too high.
By the way, I calculate the distance by converting both my current position and target position to X, Y, Z values in WGS84 coordinates (each component is in meters), sometimes this is called ECEF. Then I use the Pythagorean theorem to find the distance. Getting the X, Y, and Z values is tricky and I can share some python with you, if you'd like. But I use the tricks from the n-vector page for the heavy lifting.
@burak-yildizoz I'm not sure what you mean by:
Try using mavros_msgs::Altitude::amsl as home altitude.
and
Set home_alt to altitude when gps_received becomes true
Does the drone's home altitude have something to do with how GeoPoseStamped messages are interpreted? When I run rostopic echo mavros/home_position/home Here's what I see
geo:
latitude: 47.3978435
longitude: 8.5458706
altitude: 535.3505773470537
position:
x: 19.823331832885742
y: 10.315902709960938
z: 0.02930159494280815
Should I change position.z to 535?
@murphym18 I have pointed out this problem in GPS altitude is different in subscriber and receiver. It seems like someone needs to update the documentation. For now, issues are the documentation.
Full code
#include <ros/ros.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <sensor_msgs/NavSatFix.h>
#include <geographic_msgs/GeoPoseStamped.h>
#include <mavros_msgs/Altitude.h>
#define ALT 30 // fly altitude in meters
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg) {
current_state = *msg;
}
double latitude, longitude;
bool gps_received = false;
void get_global_pose (const sensor_msgs::NavSatFix::ConstPtr& msg) {
latitude = msg->latitude;
longitude = msg->longitude;
gps_received = true;
}
double altitude;
void get_altitude(const mavros_msgs::Altitude::ConstPtr &msg)
{
altitude = msg->amsl;
}
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::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 sub_global_pose = nh.subscribe<sensor_msgs::NavSatFix>
("mavros/global_position/global", 10, get_global_pose);
ros::Subscriber sub_altitude = nh.subscribe<mavros_msgs::Altitude>
("mavros/altitude", 10, get_altitude);
ros::Publisher pub_global = nh.advertise<geographic_msgs::GeoPoseStamped>
("mavros/setpoint_position/global", 10);
//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();
}
ROS_INFO("Connected to PX4!");
// wait for GPS connection
while(ros::ok() && !gps_received) {
ros::spinOnce();
rate.sleep();
}
// home position
const double home_lat = latitude;
const double home_lon = longitude;
const double home_alt = altitude;
ROS_INFO_ONCE("GPS Received!\nlat:%f\tlon:%f\talt:%f", home_lat, home_lon, home_alt);
geographic_msgs::GeoPoseStamped gsp;
gsp.pose.position.latitude = home_lat;
gsp.pose.position.longitude = home_lon;
gsp.pose.position.altitude = home_alt;
// send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i) {
gsp.header.stamp = ros::Time::now();
pub_global.publish(gsp);
ros::spinOnce();
rate.sleep();
}
// simulation requirements
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
// DO NOT FORGET TO CHANGE THIS BEFORE REAL FLIGHT
const bool simulation = false;
if (simulation) {
if (current_state.mode != "OFFBOARD") {
set_mode_client.call(offb_set_mode);
if (offb_set_mode.response.mode_sent)
ROS_INFO("Offboard enabled");
else
ROS_WARN("Offboard failed");
}
if (!current_state.armed) {
arming_client.call(arm_cmd);
if (arm_cmd.response.success)
ROS_INFO("Vehicle armed");
else
ROS_WARN("Arm failed");
}
}
while (ros::ok()) {
gsp.pose.position.latitude = home_lat;
gsp.pose.position.longitude = home_lon;
gsp.pose.position.altitude = home_alt + ALT;
gsp.header.stamp = ros::Time::now();
pub_global.publish(gsp);
ROS_INFO_THROTTLE(2, "\n"
"altitude\t: %.2f\n"
"target_alt\t: %.2f",
altitude, gsp.pose.position.altitude
);
ros::spinOnce();
rate.sleep();
}
return 0;
}
Hi @burak-yildizoz, Thank you so much for the quick response and the code. I have a couple of doubts though. I am trying to do the same in ardupilot by changing the mode to guided but the drone is not taking off to the desired altitude. I can do that with a takeoff client but have you implemented any logic for navigating it to multiple waypoints? Also, do we need to write a logic for checking if the waypoint is reached, or can we directly pass in waypoints one by one by using geographic_msgs::geoposestamped?
Hi murphym18, Thank you for your comment. I am also having similar issues with mavros/global_position/global topic. Are you trying to implement a multiple waypoint mission in the WGS84? Could you share the python code for checking the distance between the current position and target position? I am trying to check that with the waypoint_reached function using the Haversine formula but it's not working.
@murphym18 I have pointed out this problem in GPS altitude is different in subscriber and receiver. It seems like someone needs to update the documentation. For now, issues are the documentation.
Full code
@burak-yildizoz I checked out #1414. Do you think mavros_msgs::GlobalPositiontarget has a problem or it can work if the altitude issues are solved by using another subscriber callback for the altitude like you did? Or using geographic_msgs::Geoposestamped is the only option?
Hi murphym18, Thank you for your comment. I am also having similar issues with mavros/global_position/global topic. Are you trying to implement a multiple waypoint mission in the WGS84? Could you share the python code for checking the distance between the current position and target position? I am trying to check that with the waypoint_reached function using the Haversine formula but it's not working.
I can share the code we use to check the distance. We created a math lib in python. Here is the repo. To find the distance from this example:
LATITUDE LONGITUDE ALTITUDE
current position: 47.3978429, 8.545869, 597.5515792976987
target position: 47.39784293752265, 8.545869057172045, 550.3417375283871
Here is the python:
from droneresponse_mathtools import Lla
current_pos = Lla(47.3978429, 8.545869, 597.5515792976987)
target_pos = Lla(47.39784293752265, 8.545869057172045, 550.3417375283871)
distance = current_pos.distance(target_pos)
assert 47.21 - distance < 0.001
The complete source code for the lib is here (don't worry it's not too long). And here is the most relevant excerpt :
def distance(self, other):
p1 = self.to_pvector().as_array()
p2 = other.to_pvector().as_array()
resid = p1 - p2
resid_sq = resid ** 2
resid_sum_sq = resid_sq.sum()
dist = np.sqrt(resid_sum_sq)
return dist
The code converts both points to p-vectors. You can think of a p-vector as an X, Y, and Z value that specifies a position relative to Earth (the "p" in p-vector is for position). Here are the details about the coordinate system:
- The origin of this coordinate system is at the center of mass of Earth.
- The X-axis goes through latitude 0, longitude 0,
- The Z-axis goes through the North pole (the axis of rotation)
- The Y-axis is derived using right-hand rule (it goes through latitude 0, longitude 90).
The line of code that says p1 - p2 finds the vector that points from p2 to p1. Then we find the magnitude of that vector by squaring all the elements, adding them up and finding the square root. If you've never seen this before, we're implementing the euclidean distance.
In case it's helpful, current_pos.to_pvector() creates something like a vec3 with these values:
(4277613.251663816, 642794.9997351523, 4672256.91285744)
And target_pos.to_pvector() creates something like a vec3 with these values:
(4277581.646276147, 642790.2547726352, 4672222.165857614)
The units are in meters.
If you calculated the euclidean distance by hand you'd find the points are about 47.2 meters apart.
If you want something similar in C++ the n-vector page has a download link for a C++ lib. Given a latitude, longitude, and altitude, you can calculate a p-vector in C++ using the lib. You can use function 1 on the n-vector page to get an n-vector. Once you have the n-vector you can then calculate a p-vector using function 5. If you're not sure, you can see how it's done in the python code I linked. Or you can study the n-vector page if you want the complete details.
Are you trying to implement a multiple waypoint mission in the WGS84?
I'm working on some code that has a drone search along a path of way-points until it finds someone, then it will follow the person. Another program actually generates the search path and sends it to us. So when we get the list of way points we start off by fly to each one. In that sense it's like we're flying a mission. But we need to use set points because at any time, we might need to change course.
I think I'm going to solve the altitude problem by undoing the ASML to ellipsoid height conversion found here and here. I looked at how MAVROS calculates the ellipsoid height, and found the code here. I've not decided how I'm going to do the EGM96 conversion yet. This gist looks like a promising approach.
But I think I want to avoid getting my position from more than one message. I don't want to get my latitude and longitude from mavros/global_position/global while my altitude comes from another message. The reason is I don't want to piece together my position by looking at the time stamp of each message and combining the data of one message with interpolated/extrapolated data from another. To do that well, I'd need to re-create much of the estimator logic and that's a pretty big undertaking. To be fair, this might not be a problem. Perhaps I could combine the data from more than one message without any complicated logic so long as the time stamps are close enough (though I don't know what time delta is close enough). And I don't know what I'd do if the time values drifted to far apart. They probably wouldn't drift, but who knows.
~~I think it would be better for everyone if MAVROS just added an AMSL field to the mavros/global_position/global message. I'm even considering forking the project just for this one tiny change that is essential for my use case (and from what I gather on GitHub, it looks like I'm not the only one who needs this).~~
Anyway, I hope that helps.
Edit: I had some time to think about the problem. In some ways it seems unfortunate that the drone accepts setpoints where the altitude is specified in meters AMSL (using EGM96) but the altitude gets reported as meters above the ellipsoid. I've been looking into this problem for the past few days and it is more complicated than I thought. It's possible that this is one of the least bad ways to do it.
I found this video helpful. It provides lots of practical information and explains the difference between ellipsoid height and AMSL. After watching it, I've concluded that the drone's onboard GPS probably reports altitude in meters above MSL (using EGM96 to figure out where MSL is). According to the video, that's what most inexpensive and consumer-grade GPS units do.
This leads me to recognize that I want all my distance calculations to be done with the altitudes specified as meters above the ellipsoid's surface (assuming I continue to do distance calculations in ECEF coordinates). I don't think I'd have any problems if I calculated distance when both altitudes are specified in meters AMSL. It's probably fine so long as the points are reasonably nearby. However, I would expect to have problems if the distances were too big. And to avoid problems all together, I will make sure both points have their altitudes specified as meters above the ellipsoid. With that said, I think I'm going to hide all this from the rest of my application and use meters AMSL for all interactions with other subsystems.
Thanks a lot for the detailed explanation, @murphym18. I'll get back to you after reading, researching, and implementing the stuff you just mentioned. In the meantime, let me know if you figure out navigating to multiple waypoints. I'll also try to find a workaround.
I also think sensor_msgs::Natsatfix should provide all the 3 fields according to the drone's position in real-time as it seems quite noncoherent writing another subscriber callback just for the altitude.
Hi everyone,
I am currently facing similar issues as described above. I am not able to send any right /mavaros/setpoin_raw/target_global I always get lat=0, long=0 and alt=0 except at auto.takeoff, where I also get the altitude mismatch.
Currently I tried sending geographic_msgs::GeoPoseStamped message through /mavaros/setpoin_position/global and mavros_msgs::GlobalPositionTarget through /mavaros/setpoin_raw/global with same results... I have tried ROS Melodic with Ubuntu 18 and mavros/bionic 1.8.0-1bionic.20210505.151420 and ROS Noetic with Ubuntu 20 and mavros/focal,1.8.0-1focal.20210505.164222. With ARM64 and AMD64 architectures, in simulation and on flights...
Is there any results from your side?
Thanks in advance!
Currently I tried sending
geographic_msgs::GeoPoseStampedmessage through/mavaros/setpoin_position/globalandmavros_msgs::GlobalPositionTargetthrough/mavaros/setpoin_raw/globalwith same results
I'm not sure this helps but I noticed a typo in /mavaros/setpoin_position/global. I recognize that it could be correct in your code but I thought I'd point that out just in case.
In case it helps, the rest of this comment explains how we do what you're describing. We use ubuntu 20.04 and ROS Noetic.
We're able to use geographic_msgs::GeoPoseStamped messages with mavros. We publish those messages to /mavros/setpoint_position/global. Here are some excerpts:
How we build geographic_msgs::GeoPoseStamped messages
from geographic_msgs.msg import GeoPoseStamped
from tf.transformations import quaternion_from_euler
def _build_global_setpoint2(latitude, longitude, altitude, yaw=0.0):
"""Builds a message for the /mavros/setpoint_position/global topic
"""
geo_pose_setpoint = GeoPoseStamped()
geo_pose_setpoint.header.stamp = rospy.Time.now()
geo_pose_setpoint.pose.position.latitude = latitude
geo_pose_setpoint.pose.position.longitude = longitude
geo_pose_setpoint.pose.position.altitude = altitude
roll = 0.0
pitch = 0.0
q = quaternion_from_euler(roll, pitch, yaw)
geo_pose_setpoint.pose.orientation.x = q[0]
geo_pose_setpoint.pose.orientation.y = q[1]
geo_pose_setpoint.pose.orientation.z = q[2]
geo_pose_setpoint.pose.orientation.w = q[3]
return geo_pose_setpoint
How we publish the messages:
We publish those messages with rospy using this Publisher:
import rospy
_setpoint_global_pub = rospy.Publisher(uav_name + "/mavros/setpoint_position/global", GeoPoseStamped, queue_size=1)
setpoint = _build_global_setpoint2(1, 2, 3) # just an example setpoint
_setpoint_global_pub.publish(setpoint)
We namespace mavros nodes by UAV name. But if you're flying one drone, your code might be simpler.
The way we repeatedly send setpoint messages is beyond the scope of this comment. But hopefully that helps.
@murphym18 thanks! it's a typo while I was writing here, my bad, I wrote it right in my code.
I have the code like that -in C++, but is equivalent- and I am publishing at 10Hz rate. Is there a reason why you don't add the header of the GeoPoseStamped?
Thanks again :)
do not forget to fill header.stamp for any *Stamped message.
@vooon Thanks! I will make that change.
Is there a reason why you don't add the header of the GeoPoseStamped?
@IbonDLucas I think that was just an unintentional omission
@vooon Thanks again! I edited my earlier GitHub comment and I changed the code in my project. Turns out, this thread is showing up when I run web searches related to this topic. I figured it's better to replace the incorrect code with better code so that others will have an easier time finding examples. Thanks for the help.