Skip to content

Commit

Permalink
Merge pull request #279 from aerostack2/230-gps_support_for_ground_tr…
Browse files Browse the repository at this point in the history 
…uth_plugin

Add gps support for ground truth plugin
  • Loading branch information
@miferco97
miferco97 committed Jun 14, 2023
2 parents f9e52fa + 4bd270f commit cde667b
Show file tree
Hide file tree
Showing 3 changed files with 142 additions and 26 deletions.
13 changes: 7 additions & 6 deletions as2_state_estimator/include/as2_state_estimator/plugin_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -159,13 +159,14 @@ class StateEstimatorBase {
bool convert_earth_to_baselink_2_odom_to_baselink_transform(
const tf2::Transform& earth_to_baselink,
tf2::Transform& odom_to_baselink,
const tf2::Transform& earth_to_map,
const tf2::Transform& map_to_odom = tf2::Transform::getIdentity()) {
tf2::Transform earth_to_map;
if (!get_earth_to_map_transform(earth_to_map)) {
RCLCPP_WARN(node_ptr_->get_logger(),
"Failed to get earth to map transform, using identity transform");
return false;
}
// tf2::Transform earth_to_map;
// if (!get_earth_to_map_transform(earth_to_map)) {
// RCLCPP_WARN(node_ptr_->get_logger(),
// "Failed to get earth to map transform, using identity transform");
// return false;
// }
odom_to_baselink = map_to_odom.inverse() * earth_to_map.inverse() * earth_to_baselink;
return true;
}
Expand Down
4 changes: 3 additions & 1 deletion as2_state_estimator/plugins/ground_truth/config/default_state_estimator.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
/**:
ros__parameters:
use_ignition_tf: False
use_ignition_tf: False
use_gps: false
set_origin_on_start: false
151 changes: 132 additions & 19 deletions as2_state_estimator/plugins/ground_truth/include/ground_truth.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,19 +37,44 @@
#ifndef __GROUND_TRUTH_H__
#define __GROUND_TRUTH_H__

#include <as2_core/names/services.hpp>
#include <as2_core/utils/gps_utils.hpp>
#include <as2_core/utils/tf_utils.hpp>
#include "as2_state_estimator/plugin_base.hpp"
#include <as2_msgs/srv/get_origin.hpp>
#include <as2_msgs/srv/set_origin.hpp>
#include <as2_state_estimator/plugin_base.hpp>
#include <geographic_msgs/msg/geo_point.hpp>
#include <regex>

namespace ground_truth {

class Plugin : public as2_state_estimator_plugin_base::StateEstimatorBase {
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_;
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_sub_;

rclcpp::Service<as2_msgs::srv::SetOrigin>::SharedPtr set_origin_srv_;
rclcpp::Service<as2_msgs::srv::GetOrigin>::SharedPtr get_origin_srv_;

bool use_gps_ = false;
bool set_origin_on_start_ = false;
bool origin_param_format_ = false;
double origin_lat_ = 0.0;
double origin_lon_ = 0.0;
double origin_alt_ = 0.0;
std::string origin_param_ = "";
geometry_msgs::msg::TransformStamped earth_to_map;
geographic_msgs::msg::GeoPoint::UniquePtr origin_;
sensor_msgs::msg::NavSatFix::UniquePtr gps_pose_;
std::smatch match_;
bool using_ignition_tf_ = false;

public:
Plugin() : as2_state_estimator_plugin_base::StateEstimatorBase(){};
void on_setup() override {
node_ptr_->get_parameter("use_gps", use_gps_);
node_ptr_->get_parameter("set_origin_on_start", set_origin_on_start_);

pose_sub_ = node_ptr_->create_subscription<geometry_msgs::msg::PoseStamped>(
as2_names::topics::ground_truth::pose, as2_names::topics::ground_truth::qos,
std::bind(&Plugin::pose_callback, this, std::placeholders::_1));
Expand All @@ -58,8 +83,41 @@ class Plugin : public as2_state_estimator_plugin_base::StateEstimatorBase {
std::bind(&Plugin::twist_callback, this, std::placeholders::_1));

// publish static transform from earth to map and map to odom
geometry_msgs::msg::TransformStamped earth_to_map =
as2::tf::getTransformation(get_earth_frame(), get_map_frame(), 0, 0, 0, 0, 0, 0);
earth_to_map = as2::tf::getTransformation(get_earth_frame(), get_map_frame(), 0, 0, 0, 0, 0, 0);
if (!use_gps_) {
// TODO: MODIFY this to a initial earth to map transform (reading initial position
// from parameters or msgs )
publish_static_transform(earth_to_map);
} else {
set_origin_srv_ = node_ptr_->create_service<as2_msgs::srv::SetOrigin>(
as2_names::services::gps::set_origin,
std::bind(&Plugin::setOriginCallback, this, std::placeholders::_1,
std::placeholders::_2));
get_origin_srv_ = node_ptr_->create_service<as2_msgs::srv::GetOrigin>(
as2_names::services::gps::get_origin,
std::bind(&Plugin::getOriginCallback, this, std::placeholders::_1,
std::placeholders::_2));
gps_sub_ = node_ptr_->create_subscription<sensor_msgs::msg::NavSatFix>(
as2_names::topics::sensor_measurements::gps, as2_names::topics::sensor_measurements::qos,
std::bind(&Plugin::gps_callback, this, std::placeholders::_1));

if (set_origin_on_start_) {
node_ptr_->get_parameter("set_origin.lat", origin_lat_);
node_ptr_->get_parameter("set_origin.lon", origin_lon_);
node_ptr_->get_parameter("set_origin.alt", origin_alt_);

origin_ = std::make_unique<geographic_msgs::msg::GeoPoint>();
origin_->latitude = origin_lat_;
origin_->longitude = origin_lon_;
origin_->altitude = origin_alt_;

RCLCPP_INFO(node_ptr_->get_logger(), "Origin set to %f, %f, %f", origin_lat_, origin_lon_,
origin_alt_);
} else {
RCLCPP_INFO(node_ptr_->get_logger(), "Waiting for origin to be set");
}
}

geometry_msgs::msg::TransformStamped map_to_odom =
as2::tf::getTransformation(get_map_frame(), get_odom_frame(), 0, 0, 0, 0, 0, 0);

Expand All @@ -73,6 +131,16 @@ class Plugin : public as2_state_estimator_plugin_base::StateEstimatorBase {
};

private:
void generate_map_frame_from_gps(const geographic_msgs::msg::GeoPoint &origin,
const sensor_msgs::msg::NavSatFix &gps_pose) {
as2::gps::GpsHandler gps_handler;
gps_handler.setOrigin(origin.latitude, origin.longitude, origin.altitude);
double x, y, z;
gps_handler.LatLon2Local(gps_pose.latitude, gps_pose.longitude, gps_pose.altitude, x, y, z);
earth_to_map = as2::tf::getTransformation(get_earth_frame(), get_map_frame(), x, y, z, 0, 0, 0);
publish_static_transform(earth_to_map);
}

void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
// since it's ground_truth we consider that the frame obtained is the world frame (earth)
// so we need to publish the transform from world to base_link, with map and odom as the
Expand All @@ -84,26 +152,32 @@ class Plugin : public as2_state_estimator_plugin_base::StateEstimatorBase {
msg->header.frame_id.c_str(), get_earth_frame().c_str());
return;
}
tf2::Transform transform;
transform.setOrigin(
tf2::Vector3(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z));
transform.setRotation(tf2::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y,
msg->pose.orientation.z, msg->pose.orientation.w));
// transform.child_frame_id = get_base_frame();

auto transform = geometry_msgs::msg::TransformStamped();
transform.header.stamp = msg->header.stamp;
transform.header.frame_id = get_odom_frame();
tf2::Transform odom_to_baselink;
tf2::Transform earth_to_map_;

tf2::fromMsg(earth_to_map.transform, earth_to_map_);

convert_earth_to_baselink_2_odom_to_baselink_transform(transform, odom_to_baselink,
earth_to_map_);

auto odom_to_baselink_msg = geometry_msgs::msg::TransformStamped();
odom_to_baselink_msg.header.stamp = msg->header.stamp;
odom_to_baselink_msg.header.frame_id = get_odom_frame();
if (using_ignition_tf_) {
transform.child_frame_id = as2::tf::generateTfName("", node_ptr_->get_namespace());
odom_to_baselink_msg.child_frame_id = as2::tf::generateTfName("", node_ptr_->get_namespace());
} else {
transform.child_frame_id = get_base_frame();
odom_to_baselink_msg.child_frame_id = get_base_frame();
}
// transform.child_frame_id = get_base_frame();
transform.transform.translation.x = msg->pose.position.x;
transform.transform.translation.y = msg->pose.position.y;
transform.transform.translation.z = msg->pose.position.z;
odom_to_baselink_msg.transform = tf2::toMsg(odom_to_baselink);

transform.transform.rotation.x = msg->pose.orientation.x;
transform.transform.rotation.y = msg->pose.orientation.y;
transform.transform.rotation.z = msg->pose.orientation.z;
transform.transform.rotation.w = msg->pose.orientation.w;

publish_transform(transform);
publish_transform(odom_to_baselink_msg);

// Pose should be published in the earth frame (world)
// so we dont need to modify the frame_id
Expand All @@ -119,7 +193,46 @@ class Plugin : public as2_state_estimator_plugin_base::StateEstimatorBase {
}
publish_twist(*msg);
};
};

void getOriginCallback(const as2_msgs::srv::GetOrigin::Request::SharedPtr request,
as2_msgs::srv::GetOrigin::Response::SharedPtr response) {
if (origin_) {
response->origin = *origin_;
response->success = true;
} else {
RCLCPP_WARN(node_ptr_->get_logger(), "Origin not set");
response->success = false;
}
};

void setOriginCallback(const as2_msgs::srv::SetOrigin::Request::SharedPtr request,
as2_msgs::srv::SetOrigin::Response::SharedPtr response) {
if (origin_) {
RCLCPP_WARN(node_ptr_->get_logger(), "Origin already set");
response->success = false;
} else {
origin_ = std::make_unique<geographic_msgs::msg::GeoPoint>(request->origin);
RCLCPP_INFO(node_ptr_->get_logger(), "Origin set to %f, %f, %f", origin_->latitude,
origin_->longitude, origin_->altitude);
response->success = true;
generate_map_frame_from_gps(request->origin, *gps_pose_);
}
};

void gps_callback(sensor_msgs::msg::NavSatFix::UniquePtr msg) {
// This sould only be called when the use_gps_origin is true
if (gps_pose_) {
gps_sub_.reset();
return;
}
gps_pose_ = std::move(msg);

RCLCPP_INFO(node_ptr_->get_logger(), "GPS Callback: Map GPS pose set to %f, %f, %f",
gps_pose_->latitude, gps_pose_->longitude, gps_pose_->altitude);

generate_map_frame_from_gps(*origin_, *gps_pose_);
};

}; // class GroundTruth
}; // namespace ground_truth
#endif // __GROUND_TRUTH_HPP__

0 comments on commit cde667b

Please sign in to comment.