From cb8fafe680f31e69fd85f44c3c9e267f031a6c3d Mon Sep 17 00:00:00 2001 From: natmol Date: Wed, 6 Sep 2023 13:30:51 +0200 Subject: [PATCH] Bugfix/update map origin with home position (#1892) * Update map origin with home position * Uncrustify * Revert "Uncrustify" This reverts commit f1387c79c7670cc241986586436e3da43842e877. * Change to relative topic --------- Co-authored-by: Natalia Molina --- mavros/src/plugins/global_position.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp index 57cce6298..c8e35f395 100644 --- a/mavros/src/plugins/global_position.cpp +++ b/mavros/src/plugins/global_position.cpp @@ -48,6 +48,7 @@ class GlobalPositionPlugin : public plugin::PluginBase { GlobalPositionPlugin() : PluginBase(), gp_nh("~global_position"), + hp_nh("~home_position"), tf_send(false), use_relative_alt(true), is_map_init(false), @@ -89,7 +90,7 @@ class GlobalPositionPlugin : public plugin::PluginBase { // home position subscriber to set "map" origin // TODO use UAS - hp_sub = gp_nh.subscribe("home", 10, &GlobalPositionPlugin::home_position_cb, this); + hp_sub = hp_nh.subscribe("home", 10, &GlobalPositionPlugin::home_position_cb, this); // offset from local position to the global origin ("earth") gp_global_offset_pub = gp_nh.advertise("gp_lp_offset", 10); @@ -108,6 +109,7 @@ class GlobalPositionPlugin : public plugin::PluginBase { private: ros::NodeHandle gp_nh; + ros::NodeHandle hp_nh; //node handler in home_position namespace ros::Publisher raw_fix_pub; ros::Publisher raw_vel_pub;