Global position setpoint plugin #764

mzahana · 2017-08-06T11:42:02Z

This PR adds a plugin that allows to set global (GPS) setpoints. It accepts latitude/longitude in degrees, yaw, and local altitude. Then, it converts this to the required changes in local NED frame before it sends it to the FCU. This is tested with PX4 firmware in SITL with Gazebo.

TSC21 · 2017-08-06T12:18:37Z

@mzahana I will review this asap. But from a first look, I consider this to be integrated on setpoint_position plugin and not creating a new plugin for this. Also I don't know why you didn't use the Geographiclib methods for this as I told you about.

TSC21

Structure looks good but there are two main things to change:

Merge it with the setpoint_position as the purpose is the same.
Use GeographicLib capabilities to apply the frame conversions.

TSC21 · 2017-08-06T17:34:13Z

mavros/CMakeLists.txt

@@ -130,6 +130,7 @@ add_library(mavros_plugins
  src/plugins/hil_controls.cpp
  src/plugins/hil_actuator_controls.cpp
  src/plugins/home_position.cpp
+  src/plugins/setpoint_global_position.cpp


No need to create new plugin. You can add the methods on the setpoint_position plugin.

I have counterpoint opinion :) Let it be plugin.

@vooon this is not a setpoint_global_position, as there is a specific msg for that in Mavlink. This is a setpoint for local position as we have in setpoint_position but with a function to convert from GPS coordinates to local frame. So it is an extension of local setpoints and not a plugin for global setpoints. It will only make sense to add a new plugin for global position setpoints if those are published in a global frame.

Make sense, but keeping it separate might help with later support.
But perhaps with Geolib most code gone, so it is ok to extend sp-pos.

But perhaps with Geolib most code gone, so it is ok to extend sp-pos.

70% of the code believe me ;)

TSC21 · 2017-08-06T17:37:30Z

mavros/src/plugins/setpoint_global_position.cpp

+	 * @param target_lat. Target latitude in deg
+	 * @param target_long. Target longitude in deg
+	 */
+	 void lla2ned(double curr_lat, double curr_long, double target_lat, double target_long){


Why a new function like this? Did you check how fake_gps handles the conversion from geodetic to NED coordinates? It is everything there. You don't need to create a new function like this while you can use GeographicLib functions to do that job.

TSC21 · 2017-08-06T17:38:22Z

mavros/src/plugins/setpoint_global_position.cpp

+		lla2ned(gps_msg.latitude, gps_msg.longitude, req->latitude, req->longitude);
+		Eigen::Vector3d delta_pos(dx,dy,0.0);// dx/dy in ENU
+		/* to ENU */
+		delta_pos = ftf::transform_frame_ned_enu(delta_pos);


Again, check fake_gps

TSC21 · 2017-08-06T17:41:21Z

mavros/src/plugins/setpoint_global_position.cpp

+	void gps_cb(const sensor_msgs::NavSatFix::ConstPtr &msg){
+		gps_msg.latitude = msg->latitude;
+		gps_msg.longitude = msg->longitude;
+		gps_msg.altitude = msg->altitude;


Also is your GPS giving the altitude on AMSL or is it the height over the WGS-84 ellipsoid? You need to add a parameter that considers the second as the NavSatFix considers the height over the ellipsoid. Agaim check fake_gps and global_position plugins to see how to convert from one altitude to another.

vooon · 2017-08-08T07:14:33Z

mavros/CMakeLists.txt

@@ -130,6 +130,7 @@ add_library(mavros_plugins
  src/plugins/hil_controls.cpp
  src/plugins/hil_actuator_controls.cpp
  src/plugins/home_position.cpp
+  src/plugins/setpoint_global_position.cpp


I have counterpoint opinion :) Let it be plugin.

vooon · 2017-08-08T07:15:31Z

mavros/launch/px4_pluginlists.yaml

@@ -8,4 +8,5 @@ plugin_blacklist:
 - rangefinder

 plugin_whitelist: []
-#- 'sys_*'
+# 'sys_*'
+


Not needed change. I assume squash and commit merge.

vooon · 2017-08-08T07:18:39Z

mavros/src/plugins/setpoint_global_position.cpp

+
+#define M_DEG_TO_RAD 0.01745329251994
+#define CONSTANTS_RADIUS_OF_EARTH 6371000.0
+//#define DBL_EPSILON 2.2204460492503131E-16


I think constexpr double are preferable than prepro.
You may place it inside class or inside std_plugins.

namespace std_plugins { static constexpr double RADIUS_OR_EARTH = 6371000.0;

Deg-to-rad may be done by <angles.h> library (already used in some plugins).
And if you decide to use constant, please write pi/180.0 instead of that magic value.

All of these functions are uneeded if GeographicLib function to convert from LLA to ECEF and then apply the transform to ENU is used.

vooon · 2017-08-08T07:25:52Z

mavros/src/plugins/setpoint_global_position.cpp

+public:
+	SetpointGlobalPositionPlugin() : PluginBase(),
+		// NOTE not private handle, because we need to access gps topic, which is not under this node. Is it OK?
+		sp_nh("~")


You may use two NodeHandle, but that is fine.

vooon · 2017-08-08T07:28:55Z

mavros/src/plugins/setpoint_global_position.cpp

+	void local_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){
+		local_pose_msg.pose.position.x = msg->pose.position.x;
+		local_pose_msg.pose.position.y = msg->pose.position.y;
+		local_pose_msg.pose.position.z = msg->pose.position.z;


If all what you need is position vector, why not to use Eigen::Vector3d?

Makes sense also.

vooon · 2017-08-08T07:31:23Z

mavros/src/plugins/setpoint_global_position.cpp

+						Eigen::Vector3d::Zero(),
+						yaw, 0.0);
+
+		}


Add something like that:

else { ROS_WARN_TROTTLE_NAMED("spgp", 10, "SPG: sp not sent."); }

TSC21 · 2017-08-13T16:00:07Z

@mzahana would you mind apply the proposed changes and rebase so this can be reviewed again and merged? Thank you

TSC21 · 2017-08-17T14:56:57Z

@mzahana we would like to include this on the 0.20 release. Would you mind making the changes above?

TSC21 · 2017-11-29T16:56:58Z

@mzahana any chance to get this finished?

mzahana · 2017-11-29T21:00:33Z

Yes. I will have it on my tomorrow's schedule. Sorry for the delay, but I am graduating soon.

…

On Nov 29, 2017 7:57 PM, "Nuno Marques" ***@***.***> wrote: @mzahana <https://github.com/mzahana> any chance to get this finished? — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#764 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AIZT6-tcgqtQ3TwTGbA5AutUdGxd2AH-ks5s7YzdgaJpZM4OutW6> .

TSC21 · 2017-11-29T21:16:22Z

Yes. I will have it on my tomorrow's schedule. Sorry for the delay, but I
am graduating soon.

Awesome! Thanks and good luck!

…setpoint_plugin

mzahana · 2017-11-30T17:02:05Z

@vooon & @TSC21 : I made modifications as requested.

moved the plugin inside setpoint_position plugin
GeographicLib is used

Tested and works as expected in PX4 SITL. Please Review.
Thanks.

danividanivi · 2017-12-12T15:17:44Z

@mzahana is this ready to use? Could you give some indications on how to do it? I have a working code publishing in mavros/setpoint_position/local x,y,z coordinates, I would like to modify it to accept gps ones.

TSC21 · 2017-12-12T15:24:03Z

@mzahana is this ready to use? Could you give some indications on how to do it? I have a working code publishing in mavros/setpoint_position/local x,y,z coordinates, I would like to modify it to accept gps ones.

@danividanivi I guess you know that the idea of this code block is to convert from GPS coordinates into local ENU coordinates and send a position setpoint, right? If yes, you need to publish LLA coordinates with mavros_msgs/GlobalPositionTarget.msg format (https://github.com/mavlink/mavros/blob/master/mavros_msgs/msg/GlobalPositionTarget.msg) to mavros/setpoint_position/global topic.

danividanivi · 2017-12-13T08:00:06Z

@TSC21 yes, that looks exactly like what I need, thank you for the explanation.
However, my gps data is filtered, I just have latitude, longitude, and altitude (AMSL).
Is this enough or are the other fields mandatory? (coordinate_frame, type_mask, velocity...)

TSC21 · 2017-12-13T08:49:43Z

Actually is irrelevant given the way the code is implemented - it is just computing the local projection of the position, so it is like the type_mask was set to 0XFF8 (4088).

danividanivi · 2017-12-14T09:47:50Z

Maybe I understood this wrong. I am publishing now in "mavros/setpoint_position/global" LLA points, rostopic echo /mavros/global_position/global returns things like this:

header: 
  seq: 2602
  stamp: 
    secs: 55
    nsecs: 600609513
  frame_id: base_link
status: 
  status: 0
  service: 1
latitude: 42.3005866
longitude: -6.7324631
altitude: 58.0295222893
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 1

I was expecting that these were translated and published into "mavros/setpoint_position/local" but

rostopic echo /mavros/setpoint_position/local
WARNING: no messages received and simulated time is active.

Should I see the translated messages here or not?
This error I receive after roslaunch px4 mavros_posix_sitl.launch may be related:

[ERROR] [1513243842.855969944, 17.250000000]: Client [/myproject_ros] wants topic /mavros/global_position/local to have datatype/md5sum [geometry_msgs/PoseStamped/d3812c3cbc69362b77dc0b19b345f8f5], but our version has [nav_msgs/Odometry/cd5e73d190d741a2f92e81eda573aca7]. Dropping connection.

TSC21 · 2017-12-14T10:16:42Z

I was expecting that these were translated and published into "mavros/setpoint_position/local" but

Why? if you send to mavros/setpoint_position/global, it then sends a setpoint position to the FCU.

[ERROR] [1513243842.855969944, 17.250000000]: Client [/myproject_ros] wants topic /mavros/global_position/local to have datatype/md5sum [geometry_msgs/PoseStamped/d3812c3cbc69362b77dc0b19b345f8f5], but our version has [nav_msgs/Odometry/cd5e73d190d741a2f92e81eda573aca7]. Dropping connection.

That means you are probably using different ROS versions (and/or MAVROS) version between publishers and subscribers. You need to be sure that this doesn't happen.

danividanivi · 2017-12-14T10:38:50Z

I understand, that can be the reason why FCU was not receiving the points.

ros-kinetic-mavros is already the newest version (0.21.5-0xenial-20171201-081620-0800).
ros-kinetic-mavros-extras is already the newest version (0.21.5-0xenial-20171201-082617-0800).

I guess I have to install 0.22 from source instead of the binary installation.

TSC21 · 2017-12-14T10:40:54Z

The current binary doesn't even have the features of this PR in. So you are likely to be messing binary mavros with source mavros atm. Uninstall the binary version and use the source version only.

rrajnidhi · 2022-08-05T08:17:54Z

@TSC21 Please dont mind if i am asking a very silly question.
So if i need to navigate to a particular global_position in offboard mode, all I need to do is send the required position to mavros/setpoint_position/global ? Do i need to transmit this global_set_point in a loop of >2Hz or I just need to send only once ?
Thanks in advance

MOHAMED ABDEL KADER and others added 13 commits January 10, 2017 18:11

fix fake gps rate

6c166e3

fix

4c8b051

fix plugin_list

dc6ced1

fix

8b7bc5a

Merge branch 'master' of https://github.com/mavlink/mavros

110989a

Merge branch 'master' of https://github.com/mavlink/mavros

c846cdd

Merge branch 'master' of https://github.com/mavlink/mavros

ad873e7

Merge branch 'master' of https://github.com/mzahana/mavros

68975c6

merge upstream

cf75680

add global position setpoint plugin

15d68a2

add plugin to CMakeList

9c42ed9

fix bugs

7cfab7f

add altitude

3c2bfdd

mzahana mentioned this pull request Aug 6, 2017

Support of GPS setpoints in OFFBOARD mode PX4/PX4-Autopilot#7552

Closed

TSC21 requested changes Aug 6, 2017

View reviewed changes

vooon requested changes Aug 8, 2017

View reviewed changes

TSC21 assigned mzahana Aug 13, 2017

TSC21 added enhancement plugin labels Aug 13, 2017

TSC21 closed this Aug 14, 2017

TSC21 reopened this Aug 14, 2017

TSC21 added this to the Version 0.21 milestone Aug 25, 2017

vooon modified the milestones: Version 0.21, Version 0.22 Sep 14, 2017

Bardo91 mentioned this pull request Sep 25, 2017

Added architecture for commanding UAV with global coordinates. grvcTeam/grvc-ual#20

Merged

mzahana added 6 commits November 30, 2017 10:10

Merge remote-tracking branch 'origin/master' into pr_global_position_…

e5b7140

…setpoint_plugin

move GPS setpoints to setpoint_position plugin

9a94bf1

fix gps setpoint subscriber name

7ffdb64

move GeographicLib::Geocentric earth inside callback

5030116

add warning msg if timestamp is not updates

da32d68

Fix ROS_WARN

af484d8

vooon approved these changes Nov 30, 2017

View reviewed changes

vooon merged commit d06f9a3 into mavlink:master Nov 30, 2017

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Global position setpoint plugin #764

Global position setpoint plugin #764

mzahana commented Aug 6, 2017

TSC21 commented Aug 6, 2017

TSC21 left a comment

TSC21 Aug 6, 2017

vooon Aug 8, 2017

TSC21 Aug 8, 2017

vooon Aug 8, 2017

TSC21 Aug 8, 2017

TSC21 Aug 6, 2017

TSC21 Aug 6, 2017

TSC21 Aug 6, 2017

vooon Aug 8, 2017

vooon Aug 8, 2017

vooon Aug 8, 2017

vooon Aug 8, 2017

TSC21 Aug 8, 2017

vooon Aug 8, 2017

vooon Aug 8, 2017

TSC21 Aug 8, 2017

vooon Aug 8, 2017

TSC21 commented Aug 13, 2017

TSC21 commented Aug 17, 2017

TSC21 commented Nov 29, 2017

mzahana commented Nov 29, 2017 via email

TSC21 commented Nov 29, 2017

mzahana commented Nov 30, 2017

danividanivi commented Dec 12, 2017

TSC21 commented Dec 12, 2017 •

edited

Loading

danividanivi commented Dec 13, 2017

TSC21 commented Dec 13, 2017

danividanivi commented Dec 14, 2017

TSC21 commented Dec 14, 2017

danividanivi commented Dec 14, 2017

TSC21 commented Dec 14, 2017

rrajnidhi commented Aug 5, 2022 •

edited

Loading

Global position setpoint plugin #764

Global position setpoint plugin #764

Conversation

mzahana commented Aug 6, 2017

TSC21 commented Aug 6, 2017

TSC21 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 commented Aug 13, 2017

TSC21 commented Aug 17, 2017

TSC21 commented Nov 29, 2017

mzahana commented Nov 29, 2017 via email

TSC21 commented Nov 29, 2017

mzahana commented Nov 30, 2017

danividanivi commented Dec 12, 2017

TSC21 commented Dec 12, 2017 • edited Loading

danividanivi commented Dec 13, 2017

TSC21 commented Dec 13, 2017

danividanivi commented Dec 14, 2017

TSC21 commented Dec 14, 2017

danividanivi commented Dec 14, 2017

TSC21 commented Dec 14, 2017

rrajnidhi commented Aug 5, 2022 • edited Loading

TSC21 commented Dec 12, 2017 •

edited

Loading

rrajnidhi commented Aug 5, 2022 •

edited

Loading