Skip to content

Commit 1bee6ee

Browse files
vooonvooon
authored
Merge pull request #1780 from snktshrma/master
guided_target: accept position-target-global-int messages
2 parents d959d63 + fe92997 commit 1bee6ee

File tree

4 files changed

+211
-0
lines changed

4 files changed

+211
-0
lines changed

‎mavros/launch/apm_config.yaml‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,14 @@ setpoint_position:
112112
child_frame_id: "target_position"
113113
rate_limit: 50.0
114114
mav_frame: LOCAL_NED
115+
116+
# guided_target
117+
guided_target:
118+
tf:
119+
listen: false # enable tf listener (disable topic subscribers)
120+
frame_id: "map"
121+
child_frame_id: "target_position"
122+
rate_limit: 50.0
115123

116124
# setpoint_velocity
117125
setpoint_velocity:

‎mavros_extras/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -96,6 +96,7 @@ add_library(mavros_extras
9696
src/plugins/vision_pose_estimate.cpp
9797
src/plugins/vision_speed_estimate.cpp
9898
src/plugins/wheel_odometry.cpp
99+
src/plugins/guided_target.cpp
99100
)
100101
add_dependencies(mavros_extras
101102
${catkin_EXPORTED_TARGETS}

‎mavros_extras/mavros_plugins.xml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -93,4 +93,7 @@
9393
<class name="mag_calibration_status" type="mavros::std_plugins::MagCalStatusPlugin" base_class_type="mavros::plugin::PluginBase">
9494
<description>Send calibration status to ROS.</description>
9595
</class>
96+
<class name="guided_target" type="mavros::extra_plugins::GuidedTargetPlugin" base_class_type="mavros::plugin::PluginBase">
97+
<description>Send guided target.</description>
98+
</class>
9699
</library>

‎mavros_extras/src/plugins/guided_target.cpp‎

Lines changed: 199 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,199 @@
1+
/**
2+
* @brief Guided target plugin
3+
* @file guided_target.cpp
4+
* @author Randy Mackay <[email protected]> , Sanket Sharma <[email protected]>
5+
*
6+
* @addtogroup plugin
7+
* @{
8+
*/
9+
/*
10+
* Copyright 2022 Sanket Sharma, Randy Mackay.
11+
*
12+
* This file is part of the mavros package and subject to the license terms
13+
* in the top-level LICENSE file of the mavros repository.
14+
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
15+
*/
16+
17+
#include <mavros/mavros_plugin.h>
18+
#include <mavros/setpoint_mixin.h>
19+
#include <eigen_conversions/eigen_msg.h>
20+
21+
#include <geometry_msgs/PoseStamped.h>
22+
23+
#include <mavros_msgs/SetMavFrame.h>
24+
#include <geographic_msgs/GeoPoseStamped.h>
25+
26+
#include <GeographicLib/Geocentric.hpp>
27+
28+
#include <mavros_msgs/PositionTarget.h>
29+
#include <geographic_msgs/GeoPointStamped.h>
30+
#include <mavros_msgs/GlobalPositionTarget.h>
31+
32+
namespace mavros {
33+
namespace extra_plugins {
34+
35+
/**
36+
* @brief guided target plugin
37+
*
38+
* Send and receive setpoint positions from FCU controller.
39+
*/
40+
class GuidedTargetPlugin : public plugin::PluginBase,
41+
private plugin::SetPositionTargetLocalNEDMixin<GuidedTargetPlugin>,
42+
private plugin::SetPositionTargetGlobalIntMixin<GuidedTargetPlugin>,
43+
private plugin::TF2ListenerMixin<GuidedTargetPlugin> {
44+
public:
45+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46+
47+
GuidedTargetPlugin() : PluginBase(),
48+
sp_nh("~guided_target"),
49+
spg_nh("~"),
50+
tf_listen(false),
51+
tf_rate(50.0),
52+
is_map_init(false)
53+
{ }
54+
55+
void initialize(UAS &uas_) override
56+
{
57+
PluginBase::initialize(uas_);
58+
59+
// tf params
60+
sp_nh.param("tf/listen", tf_listen, false);
61+
sp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
62+
sp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "target_position");
63+
sp_nh.param("tf/rate_limit", tf_rate, 50.0);
64+
65+
// Publish targets received from FCU
66+
setpointg_pub = spg_nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
67+
68+
69+
// Subscriber for global origin (aka map origin).
70+
gp_origin_sub = spg_nh.subscribe("global_position/gp_origin", 10, &GuidedTargetPlugin::gp_origin_cb, this);
71+
}
72+
73+
Subscriptions get_subscriptions() override
74+
{
75+
return {
76+
make_handler(&GuidedTargetPlugin::handle_position_target_global_int)
77+
};
78+
}
79+
80+
private:
81+
82+
83+
ros::NodeHandle sp_nh;
84+
ros::NodeHandle spg_nh; //!< to get local position and gps coord which are not under sp_h()
85+
86+
ros::Subscriber gp_origin_sub; //!< global origin LLA
87+
88+
ros::Publisher setpointg_pub; //!< global position target from FCU
89+
90+
/* Stores current gps state. */
91+
//sensor_msgs::NavSatFix current_gps_msg;
92+
Eigen::Vector3d current_gps; //!< geodetic coordinates LLA
93+
Eigen::Vector3d current_local_pos; //!< Current local position in ENU
94+
95+
Eigen::Vector3d map_origin {}; //!< oigin of map frame [lla]
96+
Eigen::Vector3d ecef_origin {}; //!< geocentric origin [m]
97+
98+
uint32_t old_gps_stamp = 0; //!< old time gps time stamp in [ms], to check if new gps msg is received
99+
100+
std::string tf_frame_id;
101+
std::string tf_child_frame_id;
102+
103+
bool tf_listen;
104+
double tf_rate;
105+
bool is_map_init;
106+
107+
double arr[2] = {0, 0};
108+
109+
/* -*- mid-level helpers -*- */
110+
111+
/**
112+
* @brief Send setpoint to FCU position controller.
113+
*
114+
* @warning Send only XYZ, Yaw. ENU frame.
115+
*/
116+
117+
118+
/**
119+
* global origin in LLA
120+
*/
121+
void gp_origin_cb(const geographic_msgs::GeoPointStamped::ConstPtr &msg)
122+
{
123+
ecef_origin = {msg->position.latitude, msg->position.longitude, msg->position.altitude};
124+
/**
125+
* @brief Conversion from ECEF (Earth-Centered, Earth-Fixed) to geodetic coordinates (LLA)
126+
*/
127+
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
128+
try {
129+
earth.Reverse(ecef_origin.x(), ecef_origin.y(), ecef_origin.z(),
130+
map_origin.x(), map_origin.y(), map_origin.z());
131+
}
132+
catch (const std::exception& e) {
133+
ROS_WARN_STREAM("setpoint: Caught exception: " << e.what() << std::endl);
134+
return;
135+
}
136+
is_map_init = true;
137+
}
138+
139+
140+
141+
142+
/* -*- rx handler -*- */
143+
144+
/**
145+
* @brief handle POSITION_TARGET_GLOBAL_INT mavlink msg
146+
* handles and publishes position target received from FCU
147+
*/
148+
149+
void handle_position_target_global_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::POSITION_TARGET_GLOBAL_INT &position_target)
150+
{
151+
/* check if type_mask field ignores position*/
152+
if (position_target.type_mask & (mavros_msgs::GlobalPositionTarget::IGNORE_LATITUDE | mavros_msgs::GlobalPositionTarget::IGNORE_LONGITUDE) > 0) {
153+
ROS_WARN_NAMED("setpoint", "lat and/or lon ignored");
154+
return;
155+
}
156+
157+
/* check origin has been set */
158+
if (!is_map_init) {
159+
ROS_WARN_NAMED("setpoint", "PositionTargetGlobal failed because no origin");
160+
}
161+
162+
/* convert lat/lon target to ECEF */
163+
Eigen::Vector3d pos_target_ecef {}; //!< local ECEF coordinates on map frame [m]
164+
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
165+
try {
166+
earth.Forward(position_target.lat_int / 1E7, position_target.lon_int / 1E7, position_target.alt / 1E3,
167+
pos_target_ecef.x(), pos_target_ecef.y(), pos_target_ecef.z());
168+
}
169+
catch (const std::exception& e) {
170+
ROS_WARN_STREAM("setpoint: Caught exception: " << e.what() << std::endl);
171+
return;
172+
}
173+
174+
/* create position target PoseStamped message */
175+
auto pose = boost::make_shared<geometry_msgs::PoseStamped>();
176+
pose->header = m_uas->synchronized_header("map", position_target.time_boot_ms);
177+
pose->pose.orientation.w = 1; // unit quaternion with no rotation
178+
179+
/* convert ECEF target to ENU */
180+
const Eigen::Vector3d local_ecef = pos_target_ecef - ecef_origin;
181+
tf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef, map_origin), pose->pose.position);
182+
pose->pose.position.z = 0; // force z-axis to zero
183+
184+
/* publish target */
185+
186+
if (pose->pose.position.x != arr[0] | pose->pose.position.y != arr[1]) {
187+
setpointg_pub.publish(pose);
188+
}
189+
190+
arr[0] = pose->pose.position.x;
191+
arr[1] = pose->pose.position.y;
192+
}
193+
194+
};
195+
} // namespace std_plugins
196+
} // namespace mavros
197+
198+
#include <pluginlib/class_list_macros.h>
199+
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::GuidedTargetPlugin, mavros::plugin::PluginBase)

0 commit comments

Comments
 (0)