Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

setpoint_position: accept set-position-target-global-int messages #1184

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

setpoint_position: accept set-position-target-global-int messages #1184

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
89 changes: 86 additions & 3 deletions mavros/src/plugins/setpoint_position.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,11 @@
#include <eigen_conversions/eigen_msg.h>

#include <geometry_msgs/PoseStamped.h>
#include <geographic_msgs/GeoPointStamped.h>

#include <mavros_msgs/SetMavFrame.h>
#include <mavros_msgs/GlobalPositionTarget.h>
#include <mavros_msgs/PositionTarget.h>

#include <GeographicLib/Geocentric.hpp>
//#include <GeographicLib/Geoid.hpp>
Expand All @@ -32,7 +34,7 @@ using mavlink::common::MAV_FRAME;
/**
* @brief Setpoint position plugin
*
* Send setpoint positions to FCU controller.
* Send and receive setpoint positions from FCU controller.
*/
class SetpointPositionPlugin : public plugin::PluginBase,
private plugin::SetPositionTargetLocalNEDMixin<SetpointPositionPlugin>,
Expand All @@ -44,7 +46,8 @@ class SetpointPositionPlugin : public plugin::PluginBase,
sp_nh("~setpoint_position"),
spg_nh("~"),
tf_rate(50.0),
tf_listen(false)
tf_listen(false),
is_map_init(false)
{ }

void initialize(UAS &uas_)
Expand All @@ -71,6 +74,9 @@ class SetpointPositionPlugin : public plugin::PluginBase,
gps_sub = spg_nh.subscribe("global_position/global", 10, &SetpointPositionPlugin::gps_cb, this);
// Subscribe for current local ENU pose.
local_sub = spg_nh.subscribe("local_position/pose", 10, &SetpointPositionPlugin::local_cb, this);

// subscriber for global origin (aka map origin)
gp_origin_sub = spg_nh.subscribe("global_position/gp_origin", 10, &SetpointPositionPlugin::gp_origin_cb, this);
}
mav_frame_srv = sp_nh.advertiseService("mav_frame", &SetpointPositionPlugin::set_mav_frame_cb, this);

Expand All @@ -81,11 +87,16 @@ class SetpointPositionPlugin : public plugin::PluginBase,
} else {
mav_frame = utils::mav_frame_from_str(mav_frame_str);
}

// publish targets received from FCU
setpointg_pub = sp_nh.advertise<geometry_msgs::PoseStamped>("cmd_pos", 10);
}

Subscriptions get_subscriptions()
{
return { /* Rx disabled */ };
return {
make_handler(&SetpointPositionPlugin::handle_set_position_target_global_int),
};
}

private:
Expand All @@ -98,19 +109,24 @@ class SetpointPositionPlugin : public plugin::PluginBase,
ros::Subscriber setpointg_sub; //!< GPS setpoint
ros::Subscriber gps_sub; //!< current GPS
ros::Subscriber local_sub; //!< current local ENU
ros::Subscriber gp_origin_sub; //!< global origin LLA
ros::ServiceServer mav_frame_srv;
ros::Publisher setpointg_pub; //!< global position target from FCU

/* Stores current gps state. */
//sensor_msgs::NavSatFix current_gps_msg;
Eigen::Vector3d current_gps; //!< geodetic coordinates LLA
Eigen::Vector3d current_local_pos; //!< Current local position in ENU
Eigen::Vector3d map_origin {}; //!< oigin of map frame [lla]
Eigen::Vector3d ecef_origin {}; //!< geocentric origin [m]
uint32_t old_gps_stamp = 0; //!< old time gps time stamp in [ms], to check if new gps msg is received

std::string tf_frame_id;
std::string tf_child_frame_id;

bool tf_listen;
double tf_rate;
bool is_map_init;

MAV_FRAME mav_frame;

Expand Down Expand Up @@ -250,6 +266,27 @@ class SetpointPositionPlugin : public plugin::PluginBase,
current_local_pos = ftf::to_eigen(msg->pose.position);
}

/**
* global origin in LLA
*/
void gp_origin_cb(const geographic_msgs::GeoPointStamped::ConstPtr &msg)
{
ecef_origin = {msg->position.latitude, msg->position.longitude, msg->position.altitude};
/**
* @brief Conversion from ECEF (Earth-Centered, Earth-Fixed) to geodetic coordinates (LLA)
*/
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
try {
earth.Reverse(ecef_origin.x(), ecef_origin.y(), ecef_origin.z(),
map_origin.x(), map_origin.y(), map_origin.z());
}
catch (const std::exception& e) {
ROS_WARN_STREAM("setpoint: Caught exception: " << e.what() << std::endl);
return;
}
is_map_init = true;
}

bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res)
{
mav_frame = static_cast<MAV_FRAME>(req.mav_frame);
Expand All @@ -258,6 +295,52 @@ class SetpointPositionPlugin : public plugin::PluginBase,
res.success = true;
return true;
}

/* -*- rx handler -*- */

/**
* @brief handle SET_POSITION_TARGET_GLOBAL_INT mavlink msg
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

SET_ prefixes are usually used for messages you send to the FCU, not something you receive from. I understand that what you are doing is basically expect that the data coming from the GCS gets parsed here, since what MAVROS does is nothing more than serve as a proxy to forward messages, but we should not mess how we process data that comes from the FCU and the data that comes from the GCS. This last one should be handled separately.

If the data comes from the FCU, it needs to come in POSITION_TARGET_GLOBAL_INT messages.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Btw, for specific GCS data:

Copy link
Contributor Author

@rmackay9 rmackay9 Mar 7, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@TSC21, thanks for the feedback and links. I see what you're saying about keeping the handling of messages from the GCS separate from those received from the FCU. I was planning to actually make it possible for the vehicle to send set-position-target-local-ned messages so that ROS's navigation can help the vehicle get around some obstacles close to the vehicle.

In any case, I'll think about it a bit more and I'm still trying to find the time to resolve the frame conversion issue in this PR.

* handles and publishes position target received from FCU
*/
void handle_set_position_target_global_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SET_POSITION_TARGET_GLOBAL_INT &position_target)
{
/* check if type_mask field ignores position*/
if (position_target.type_mask & (mavros_msgs::GlobalPositionTarget::IGNORE_LATITUDE | mavros_msgs::GlobalPositionTarget::IGNORE_LONGITUDE) > 0) {
ROS_WARN_NAMED("setpoint", "lat and/or lon ignored");
return;
}

/* check origin has been set */
if (!is_map_init) {
ROS_WARN_NAMED("setpoint", "SetPositionTargetGlobal failed because no origin");
}

/* convert lat/lon target to ECEF */
Eigen::Vector3d pos_target_ecef {}; //!< local ECEF coordinates on map frame [m]
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
try {
earth.Forward(position_target.lat_int / 1E7, position_target.lon_int / 1E7, position_target.alt / 1E3,
pos_target_ecef.x(), pos_target_ecef.y(), pos_target_ecef.z());
}
catch (const std::exception& e) {
ROS_WARN_STREAM("setpoint: Caught exception: " << e.what() << std::endl);
return;
}

/* create position target PoseStamped message */
auto pose = boost::make_shared<geometry_msgs::PoseStamped>();
pose->header = m_uas->synchronized_header("map", position_target.time_boot_ms);
pose->pose.orientation.w = 1; // unit quaternion with no rotation

/* convert ECEF target to ENU */
const Eigen::Vector3d local_ecef = pos_target_ecef - ecef_origin;
tf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef, map_origin), pose->pose.position);
pose->pose.position.z = 0; // force z-axis to zero

/* publish target */
setpointg_pub.publish(pose);
}

};
} // namespace std_plugins
} // namespace mavros
Expand Down