Skip to content

Commit 7c3fcb5

Browse files
snktshrmasnktshrma
committed
Update guided_target.cpp
Update mavros_plugins.xml Update CMakeLists.txt Added offboard_position.cpp Update apm_config.yaml Update offboard_position.cpp Update offboard_position.cpp Rename offboard_position.cpp to guided_target.cpp Update CMakeLists.txt Update mavros_plugins.xml Update apm_config.yaml
1 parent cbe3aef commit 7c3fcb5

File tree

4 files changed

+204
-0
lines changed

4 files changed

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

0 commit comments

Comments
 (0)