burak-yildizoz
commented
3 years ago 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.
murphym18
abhiramdapke
IbonDLucas
vooon
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 -
For this, the subscriber is -
The function for the waypoint is -
For this, the Publisher is -
A snippet of the script that I am using is like this -
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) -
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
Log of rostopic echo /mavros/setpoint_raw/global
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!