Skip to content
Snippets Groups Projects
Commit f2cfa4ad authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

optionally publishing ecef TF

parent b5408f42
No related branches found
No related tags found
1 merge request!4Devel
...@@ -37,6 +37,7 @@ namespace wolf ...@@ -37,6 +37,7 @@ namespace wolf
class PublisherGnssTf: public Publisher class PublisherGnssTf: public Publisher
{ {
bool publish_ecef_;
std::string map_frame_id_, enu_frame_id_, ecef_frame_id_; std::string map_frame_id_, enu_frame_id_, ecef_frame_id_;
tf::StampedTransform T_enu_map_, T_ecef_enu_; tf::StampedTransform T_enu_map_, T_ecef_enu_;
tf::TransformBroadcaster tfb_; tf::TransformBroadcaster tfb_;
......
...@@ -36,18 +36,23 @@ PublisherGnssTf::PublisherGnssTf(const std::string& _unique_name, ...@@ -36,18 +36,23 @@ PublisherGnssTf::PublisherGnssTf(const std::string& _unique_name,
ProblemConstPtr _problem) : ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem) Publisher(_unique_name, _server, _problem)
{ {
sensor_gnss_ = std::static_pointer_cast<const SensorGnss>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_gnss_name"))); auto sensor = std::dynamic_pointer_cast<const SensorGnss>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_gnss_name")));
if (not sensor_gnss_)
throw std::runtime_error("PublisherGnssTf: 'sensor_gnss_name' not found or wrong sensor type.");
T_enu_map_.setIdentity(); T_enu_map_.setIdentity();
T_enu_map_.frame_id_ = "ENU"; T_enu_map_.frame_id_ = "ENU";
T_enu_map_.stamp_ = ros::Time::now(); T_enu_map_.stamp_ = ros::Time::now();
T_ecef_enu_.setIdentity();
T_ecef_enu_.frame_id_ = "ECEF";
T_ecef_enu_.child_frame_id_ = "ENU";
T_ecef_enu_.stamp_ = ros::Time::now();
T_enu_map_.child_frame_id_ = _server.getParam<std::string>(prefix_ + "/map_frame_id"); T_enu_map_.child_frame_id_ = _server.getParam<std::string>(prefix_ + "/map_frame_id");
publish_ecef_ = _server.getParam<bool>(prefix_ + "/publish_ecef");
if (publish_ecef_)
{
T_ecef_enu_.setIdentity();
T_ecef_enu_.frame_id_ = "ECEF";
T_ecef_enu_.child_frame_id_ = "ENU";
T_ecef_enu_.stamp_ = ros::Time::now();
}
} }
void PublisherGnssTf::initialize(ros::NodeHandle& nh, const std::string& topic) void PublisherGnssTf::initialize(ros::NodeHandle& nh, const std::string& topic)
...@@ -56,36 +61,32 @@ void PublisherGnssTf::initialize(ros::NodeHandle& nh, const std::string& topic) ...@@ -56,36 +61,32 @@ void PublisherGnssTf::initialize(ros::NodeHandle& nh, const std::string& topic)
void PublisherGnssTf::publishDerived() void PublisherGnssTf::publishDerived()
{ {
tf::Matrix3x3 tf_R_enu_map, tf_R_enu_ecef;
tf::Vector3 tf_t_enu_map, tf_t_enu_ecef;
if (not sensor_gnss_->isEnuDefined()) if (not sensor_gnss_->isEnuDefined())
return; return;
//Eigen::Matrix3d ei_R_enu_ecef = sensor_gnss_->getREnuEcef(); // ENU-MAP
//Eigen::Vector3d ei_t_enu_ecef = sensor_gnss_->gettEnuEcef(); tf::Vector3 tf_t_enu_map;
tf::Matrix3x3 tf_R_enu_map;
//Eigen::Matrix3d ei_R_enu_map = sensor_gnss_->getREnuMap();
//Eigen::Vector3d ei_t_enu_map = sensor_gnss_->gettEnuMap();
//tf::matrixEigenToTF(ei_R_enu_ecef, tf_R_enu_ecef);
//tf::vectorEigenToTF(ei_t_enu_ecef, tf_t_enu_ecef);
tf::matrixEigenToTF(sensor_gnss_->getREnuEcef(), tf_R_enu_ecef);
tf::vectorEigenToTF(sensor_gnss_->gettEnuEcef(), tf_t_enu_ecef);
//tf::matrixEigenToTF(ei_R_enu_map, tf_R_enu_map);
//tf::vectorEigenToTF(ei_t_enu_map, tf_t_enu_map);
tf::matrixEigenToTF(sensor_gnss_->getREnuMap(), tf_R_enu_map); tf::matrixEigenToTF(sensor_gnss_->getREnuMap(), tf_R_enu_map);
tf::vectorEigenToTF(sensor_gnss_->gettEnuMap(), tf_t_enu_map); tf::vectorEigenToTF(sensor_gnss_->gettEnuMap(), tf_t_enu_map);
T_ecef_enu_.setData(tf::Transform(tf_R_enu_ecef, tf_t_enu_ecef).inverse());
T_enu_map_ .setData(tf::Transform(tf_R_enu_map, tf_t_enu_map)); T_enu_map_ .setData(tf::Transform(tf_R_enu_map, tf_t_enu_map));
T_ecef_enu_.stamp_ = ros::Time::now();
T_enu_map_ .stamp_ = ros::Time::now(); T_enu_map_ .stamp_ = ros::Time::now();
tfb_.sendTransform(T_ecef_enu_);
tfb_.sendTransform(T_enu_map_); tfb_.sendTransform(T_enu_map_);
// ECEF-ENU
if (publish_ecef_)
{
tf::Vector3 tf_t_enu_ecef;
tf::Matrix3x3 tf_R_enu_ecef;
T_ecef_enu_.setData(tf::Transform(tf_R_enu_ecef, tf_t_enu_ecef).inverse());
T_ecef_enu_.stamp_ = ros::Time::now();
tf::matrixEigenToTF(sensor_gnss_->getREnuEcef(), tf_R_enu_ecef);
tf::vectorEigenToTF(sensor_gnss_->gettEnuEcef(), tf_t_enu_ecef);
tfb_.sendTransform(T_ecef_enu_);
}
} }
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment