diff --git a/alpha_localization/CMakeLists.txt b/alpha_localization/CMakeLists.txt index 63f315e..597de78 100644 --- a/alpha_localization/CMakeLists.txt +++ b/alpha_localization/CMakeLists.txt @@ -14,9 +14,15 @@ find_package(catkin REQUIRED COMPONENTS geographic_msgs nav_msgs tf2_eigen + tf2 + tf2_ros ) find_package(Eigen3 REQUIRED) + +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") +find_package(GeographicLib REQUIRED) + add_definitions(${EIGEN_DEFINITIONS}) catkin_package( @@ -53,8 +59,7 @@ include_directories( add_executable(depth_filter_node src/depth_filter_node.cpp) add_executable(pressure_to_depth_node src/pressure_to_depth_node.cpp) add_executable(imu_ned_to_enu src/imu_ned_enu.cpp) -add_executable(geopose_publisher_node src/geopose_publisher_node.cpp) -add_executable(gps_odom_transform_node src/gps_odom_transform.cpp) +add_executable(world_odom_transform_node src/world_odom_transform.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -67,8 +72,7 @@ add_executable(gps_odom_transform_node src/gps_odom_transform.cpp) add_dependencies(depth_filter_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(pressure_to_depth_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(imu_ned_to_enu ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(geopose_publisher_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(gps_odom_transform_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(world_odom_transform_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against target_link_libraries(depth_filter_node @@ -83,10 +87,7 @@ target_link_libraries(imu_ned_to_enu ${catkin_LIBRARIES} ) -target_link_libraries(geopose_publisher_node - ${catkin_LIBRARIES} -) -target_link_libraries(gps_odom_transform_node +target_link_libraries(world_odom_transform_node ${catkin_LIBRARIES} ) diff --git a/alpha_localization/launch/gps_odom_transform.launch b/alpha_localization/launch/gps_odom_transform.launch deleted file mode 100644 index 56fb07f..0000000 --- a/alpha_localization/launch/gps_odom_transform.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/alpha_localization/launch/world_odom_transform.launch b/alpha_localization/launch/world_odom_transform.launch new file mode 100644 index 0000000..2557593 --- /dev/null +++ b/alpha_localization/launch/world_odom_transform.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/alpha_localization/package.xml b/alpha_localization/package.xml index 6371850..3668f1b 100644 --- a/alpha_localization/package.xml +++ b/alpha_localization/package.xml @@ -21,6 +21,8 @@ geographic_msgs nav_msgs tf2_eigen + tf2 + tf2_ros diff --git a/alpha_localization/readme.md b/alpha_localization/readme.md new file mode 100644 index 0000000..963968a --- /dev/null +++ b/alpha_localization/readme.md @@ -0,0 +1,22 @@ +# A suite of localization nodes + +### Dependency +- GeographicLib +``` +sudo apt install geographiclib-tools +sudo apt install libgeographic-dev +``` + +- for magnetic model installation please check this link +``` +https://geographiclib.sourceforge.io/C++/doc/magnetic.html +``` +- download the `WMM2020` model then do the following +``` +sudo mkdir -p /usr/local/share/GeographicLib +sudo tar xofjC wmm2020.tar.bz2 /usr/local/share/GeographicLib +``` +- you may need to rename the file, e.g., +``` +sudo cp wmm2020.wmm WMM2020.wmm +``` \ No newline at end of file diff --git a/alpha_localization/src/geopose_publisher_node.cpp b/alpha_localization/src/geopose_publisher_node.cpp deleted file mode 100644 index deecda0..0000000 --- a/alpha_localization/src/geopose_publisher_node.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/* - - This project is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This project is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with the project. If not, see . - - Author: Mingxi ZHou - Email: mzhou@uri.edu - Year: 2023 - - Copyright (C) 2023 Smart Ocean Systems Laboratory - - This program subscribe to the odometry topic and convert - position into lat lon altitude using toLL service. - The values are published to a topic geolocation in - geographic_msgs/GeoPoseStamped message type -*/ - -#include "geopose_publisher_node.hpp" - -OdomToGeoposeNode::OdomToGeoposeNode() -{ - m_nh.reset(new ros::NodeHandle("")); - m_pnh.reset(new ros::NodeHandle("~")); - - m_pnh->param("toll_service", m_toll_service, "toLL"); - m_pnh->param("odometry_source", m_odom_source, "odometry/filtered/local"); - - - m_odom_subscriber = m_nh->subscribe( - m_odom_source, 10, &OdomToGeoposeNode::f_odom_callback, this); - - m_geopose_publisher = m_nh->advertise - ("odometry/geopose",10); -} - -void OdomToGeoposeNode::f_odom_callback(const nav_msgs::Odometry::ConstPtr &msg) -{ - geographic_msgs::GeoPoseStamped geopose; - //check to_LL service - if(!ros::service::exists(m_toll_service, false)) { - // std::cout << "The To_LL service is not available from " << m_toll_service << std::endl; - } - else{ - //call the service - robot_localization::ToLL ser; - ser.request.map_point.x = msg->pose.pose.position.x; - ser.request.map_point.y = msg->pose.pose.position.y; - ser.request.map_point.z = msg->pose.pose.position.z; - - if(!ros::service::call(m_toll_service, ser)) { - std::cout << "Failed to compute the latitude and longitude from odom" << std::endl; - } - else{ - //map response into our service response. - geopose.pose.position.latitude = ser.response.ll_point.latitude; - geopose.pose.position.longitude = ser.response.ll_point.longitude; - geopose.pose.position.altitude = ser.response.ll_point.altitude; - geopose.pose.orientation.x = msg->pose.pose.orientation.x; - geopose.pose.orientation.y = msg->pose.pose.orientation.y; - geopose.pose.orientation.z = msg->pose.pose.orientation.z; - geopose.pose.orientation.w = msg->pose.pose.orientation.w; - geopose.header= msg->header; - m_geopose_publisher.publish(geopose); - } - } - -} - -int main(int argc, char* argv[]) { - - ros::init(argc, argv, "odom_to_geopose"); - - OdomToGeoposeNode odom2geopose; - - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/alpha_localization/src/geopose_publisher_node.hpp b/alpha_localization/src/geopose_publisher_node.hpp deleted file mode 100644 index 0fafbc2..0000000 --- a/alpha_localization/src/geopose_publisher_node.hpp +++ /dev/null @@ -1,57 +0,0 @@ -/* - - This project is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This project is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with the project. If not, see . - - Author: Mingxi ZHou - Email: mzhou@uri.edu - Year: 2023 - - Copyright (C) 2023 Smart Ocean Systems Laboratory - - This program subscribe to the odometry topic and convert - position into lat lon altitude using toLL service. - The values are published to a topic geolocation in - geographic_msgs/GeoPoseStamped message type -*/ - -#pragma once - -#include "ros/ros.h" -#include "geographic_msgs/GeoPoseStamped.h" -#include "nav_msgs/Odometry.h" -#include "robot_localization/ToLL.h" - -class OdomToGeoposeNode{ - -private: - ros::NodeHandlePtr m_nh; - - ros::NodeHandlePtr m_pnh; - - ros::Publisher m_geopose_publisher; - - ros::Subscriber m_odom_subscriber; - - std::string m_toll_service; - - std::string m_odom_source; - - void f_odom_callback(const nav_msgs::Odometry::ConstPtr &msg); - - -public: - - OdomToGeoposeNode(); - -}; diff --git a/alpha_localization/src/gps_odom_transform.cpp b/alpha_localization/src/gps_odom_transform.cpp deleted file mode 100644 index ca2db62..0000000 --- a/alpha_localization/src/gps_odom_transform.cpp +++ /dev/null @@ -1,215 +0,0 @@ -/* - This file is part of ALPHA AUV project. - - This project is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This project is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with the project. If not, see . - - Author: Mingxi Zhou - Email: mzhou - Year: 2024 - - Copyright (C) 2024 Smart Ocean Systems Laboratory -*/ - -#include "gps_odom_transform.hpp" - -GpsOdomTransform::GpsOdomTransform(){ - m_nh.reset(new ros::NodeHandle("")); - m_pnh.reset(new ros::NodeHandle("~")); - - m_pnh->param("world", m_world_frame, "world"); - - m_pnh->param("odom", m_odom_frame, "odom"); - - m_pnh->param("tf_prefix", m_tf_prefix, ""); - - // m_pnh->param("mag_declination", m_mag_declination, 0.0); - //mag_north - true north in ENU frame. - - m_pnh->param("acceptable_var", m_acceptable_var, 0.0); - - m_pnh->param("position_accuracy", m_position_accuracy, 0.0); - - m_pnh->param("max_gps_wait_time", m_gps_wait_time, 60.0); - - m_pnh->param("datum_latitude", m_datum_latitude, 41.0); - - m_pnh->param("datum_longitude", m_datum_longitude, -71.0); - - m_pnh->param("datum_altitude", m_datum_altitude, 0.0); - - m_datum.latitude = m_datum_latitude; - m_datum.longitude = m_datum_longitude; - m_datum.altitude = m_datum_altitude; - - m_world_frame = m_tf_prefix + "/" + m_world_frame; - - m_odom_frame = m_tf_prefix + "/" + m_odom_frame; - - m_gps_odom_publisher = m_nh->advertise("gps/odometry", 10); - - m_datum_publisher = m_nh->advertise("gps/datum",10); - - - m_gps_fix_subscriber = m_nh->subscribe("gps/fix", 10, - &GpsOdomTransform::f_cb_gps_fix, this); - m_odom_subscriber = m_nh->subscribe("odometry", 10, - &GpsOdomTransform::f_cb_odom, this); - - /** - * Initialize services - */ - fromLL_server = m_pnh->advertiseService - ( - "fromLL", - std::bind(&GpsOdomTransform::f_cb_fromLL_srv, - this,std::placeholders::_1,std::placeholders::_2 - ) - ); - - toLL_server = m_pnh->advertiseService - ( - "toLL", - std::bind(&GpsOdomTransform::f_cb_toLL_srv, - this, - std::placeholders::_1,std::placeholders::_2 - ) - ); - - reset_tf_server = m_pnh->advertiseService - ( - "reset_datum", - std::bind(&GpsOdomTransform::f_cb_reset_tf_srv, - this,std::placeholders::_1,std::placeholders::_2 - ) - ); - - m_transform_listener.reset(new - tf2_ros::TransformListener(m_transform_buffer) - ); - -} - -void GpsOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) -{ - //compute the latitude longitude in the world frame using datum. - geometry_msgs::Point map_point; - nav_msgs::Odometry gps_world; - geographic_msgs::GeoPoint ll_point; - - ll_point.latitude = msg.latitude; - ll_point.longitude = msg.longitude; - ll_point.altitude = msg.altitude; - //Get x and y from lattiude and longitude. x->east, y->north - f_ll2dis(ll_point, map_point); - - //convert distance from gps into odom frame using mag_declination. - try { - auto tf_w2o = m_transform_buffer.lookupTransform( - m_odom_frame, - m_world_frame, - ros::Time::now(), - ros::Duration(1.0) - ); - Eigen::Vector3d p_world; - auto tf_eigen = tf2::transformToEigen(tf_w2o); - - p_world = tf_eigen.rotation() * - Eigen::Vector3d(map_point.x, - map_point.y, - map_point.z) - + tf_eigen.translation(); - gps_world.pose.pose.position.x = p_world.x(); - gps_world.pose.pose.position.y = p_world.y(); - // gps_world.pose.pose.position.x = cos(m_mag_declination)*map_point.x + sin(m_mag_declination)*map_point.y; - // gps_world.pose.pose.position.y =-sin(m_mag_declination)*map_point.x + cos(m_mag_declination)*map_point.y; - gps_world.header.frame_id = m_odom_frame; - gps_world.header.stamp = msg.header.stamp; - gps_world.pose.covariance[0] = pow(m_position_accuracy,2); - gps_world.pose.covariance[1] = 0; - gps_world.pose.covariance[2] = 0; - gps_world.pose.covariance[6] =0; - gps_world.pose.covariance[7] = pow(m_position_accuracy,2); - gps_world.pose.covariance[8] = 0; - gps_world.pose.covariance[12] = 0; - gps_world.pose.covariance[13] = 0; - gps_world.pose.covariance[14] = pow(m_position_accuracy,2); - - - m_gps_odom_publisher.publish(gps_world); - m_datum_publisher.publish(m_datum); - } catch(tf2::TransformException &e) { - ROS_WARN_STREAM_THROTTLE(10, std::string("Can't get the tf from world to odom") + e.what()); - } - -} - -void GpsOdomTransform::f_cb_odom(const nav_msgs::OdometryConstPtr& msg) -{ - // m_odom = *msg; -} - -bool GpsOdomTransform::f_cb_reset_tf_srv(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &resp) -{ - -} - -bool GpsOdomTransform::f_cb_fromLL_srv(robot_localization::FromLL::Request &req, robot_localization::FromLL::Response &resp) -{ - f_ll2dis(req.ll_point, resp.map_point); - return true; - -} - -bool GpsOdomTransform::f_cb_toLL_srv(robot_localization::ToLL::Request &req, robot_localization::ToLL::Response &resp) -{ - f_dis2ll(req.map_point, resp.ll_point); - return true; -} - -void GpsOdomTransform::f_ll2dis(geographic_msgs::GeoPoint ll_point, geometry_msgs::Point& map_point) -{ - double north = m_earthR*(ll_point.latitude - m_datum.latitude)/180.0*M_PI; - double east = m_earthR*cos(m_datum.latitude/180.0*M_PI) * (ll_point.longitude - m_datum.longitude)/180.0*M_PI; - map_point.x = east; - map_point.y = north; - map_point.z = ll_point.altitude; - //in world frame. - -} - -void GpsOdomTransform::f_dis2ll(geometry_msgs::Point map_point, geographic_msgs::GeoPoint& ll_point) -{ - //from world frame - double lat = m_datum.latitude + map_point.y/m_earthR * 180.0/M_PI; - double lon = m_datum.longitude + map_point.x/(m_earthR*cos(m_datum.latitude/180.0*M_PI)) * 180.0/M_PI; - ll_point.latitude = lat; - ll_point.longitude = lon; - ll_point.altitude = map_point.z; -} - - -int main(int argc, char* argv[]) { - - ros::init(argc, argv, "gps_transform"); - - GpsOdomTransform d; - - ros::spin(); - - - return 0; -} \ No newline at end of file diff --git a/alpha_localization/src/world_odom_transform.cpp b/alpha_localization/src/world_odom_transform.cpp new file mode 100644 index 0000000..f8d2566 --- /dev/null +++ b/alpha_localization/src/world_odom_transform.cpp @@ -0,0 +1,460 @@ +/* + This file is part of ALPHA AUV project. + + This project is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This project is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with the project. If not, see . + + Author: Mingxi Zhou + Email: mzhou + Year: 2024 + + Copyright (C) 2024 Smart Ocean Systems Laboratory +*/ + +#include "world_odom_transform.hpp" +#include +#include +#include + + +WorldOdomTransform::WorldOdomTransform(){ + m_nh.reset(new ros::NodeHandle("")); + m_pnh.reset(new ros::NodeHandle("~")); + + m_pnh->param("world", m_world_frame, "world"); + + m_pnh->param("odom", m_odom_frame, "odom"); + + m_pnh->param("tf_prefix", m_tf_prefix, ""); + + m_pnh->param("mag_declination", m_mag_declination, 0.0); + + m_pnh->param("mag_declination_auto", m_mag_declination_auto, true); + //mag_north - true north in ENU frame. + + m_pnh->param("acceptable_var", m_acceptable_var, 0.0); + + m_pnh->param("position_accuracy", m_position_accuracy, 0.0); + + m_pnh->param("max_gps_wait_time", m_gps_wait_time, 60.0); + + m_pnh->param("datum_latitude", m_datum_latitude, 0.0); + + m_pnh->param("datum_longitude", m_datum_longitude, 0.0); + + m_pnh->param("datum_altitude", m_datum_altitude, 0.0); + + m_pnh->param("publish_tf", m_publish_tf, true); + + m_datum.latitude = m_datum_latitude; + m_datum.longitude = m_datum_longitude; + m_datum.altitude = m_datum_altitude; + + + if(m_datum.latitude==0){ + ROS_WARN("Datum is not set please set the parameters"); + m_datum_set = false; + } + else{ + m_datum_set = true; + } + m_tf_set = false; + m_world_frame = m_tf_prefix + "/" + m_world_frame; + + m_odom_frame = m_tf_prefix + "/" + m_odom_frame; + + m_gps_odom_publisher = m_nh->advertise("gps/odometry", 10); + + m_datum_publisher = m_nh->advertise("gps/datum",10); + + m_geopose_publisher = m_nh->advertise("odometry/geopose",10); + + + m_gps_fix_subscriber = m_nh->subscribe("gps/fix", 10, + &WorldOdomTransform::f_cb_gps_fix, this); + m_odom_subscriber = m_nh->subscribe("odometry", 10, + &WorldOdomTransform::f_cb_odom, this); + + /** + * Initialize services + */ + fromLL_server = m_nh->advertiseService + ( + "fromLL", + std::bind(&WorldOdomTransform::f_cb_fromLL_srv, + this,std::placeholders::_1,std::placeholders::_2 + ) + ); + + toLL_server = m_nh->advertiseService + ( + "toLL", + std::bind(&WorldOdomTransform::f_cb_toLL_srv, + this, + std::placeholders::_1,std::placeholders::_2 + ) + ); + + reset_datum_server = m_pnh->advertiseService + ( + "reset_datum", + std::bind(&WorldOdomTransform::f_cb_reset_datum_srv, + this,std::placeholders::_1,std::placeholders::_2 + ) + ); + + reset_tf_server = m_pnh->advertiseService + ( + "reset_tf", + std::bind(&WorldOdomTransform::f_cb_reset_tf_srv, + this,std::placeholders::_1,std::placeholders::_2 + ) + ); + + m_transform_listener.reset(new + tf2_ros::TransformListener(m_transform_buffer) + ); + +} + +void WorldOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) +{ + //compute the latitude longitude in the world frame using datum. + geometry_msgs::Point map_point; + nav_msgs::Odometry gps_odom; + geographic_msgs::GeoPoint ll_point; + + m_gps = msg; + m_gps_for_datum = msg; + m_odom_gps = m_odom; //map the most recent odom; + + + if(m_datum_set) + { + m_datum_publisher.publish(m_datum); + } + // printf("Got GPS\r\n"); + //only do the following if the tf between world and odom are set. + if(m_tf_set && m_datum_set) + { + if(m_gps.position_covariance[0]-1) + { + ll_point.latitude = msg.latitude; + ll_point.longitude = msg.longitude; + ll_point.altitude = msg.altitude; + + //Get x and y from lattiude and longitude. x->east, y->north + f_ll2dis(ll_point, map_point); + //convert distance from gps into odom frame using mag_declination. + try { + auto tf_w2o = m_transform_buffer.lookupTransform( + m_odom_frame, + m_world_frame, + ros::Time(0) + ); + + geometry_msgs::PoseStamped odom_pose, world_pose; + world_pose.header = msg.header; + world_pose.header.frame_id = m_world_frame; + world_pose.pose.position.x = map_point.x; + world_pose.pose.position.y = map_point.y; + world_pose.pose.position.z = map_point.z; + world_pose.pose.orientation.x = 0; + world_pose.pose.orientation.y = 0; + world_pose.pose.orientation.z = 0; + world_pose.pose.orientation.w = 1.0; + // Transform the pose from odom frame to world frame + tf2::doTransform(world_pose, odom_pose, tf_w2o); + + auto tf_eigen = tf2::transformToEigen(tf_w2o); + + gps_odom.pose.pose.position.x = odom_pose.pose.position.x; + gps_odom.pose.pose.position.y = odom_pose.pose.position.y; + gps_odom.header.frame_id = m_odom_frame; + gps_odom.header.stamp = msg.header.stamp; + gps_odom.pose.covariance[0] = pow(m_position_accuracy,2); + gps_odom.pose.covariance[1] = 0; + gps_odom.pose.covariance[2] = 0; + gps_odom.pose.covariance[6] =0; + gps_odom.pose.covariance[7] = pow(m_position_accuracy,2); + gps_odom.pose.covariance[8] = 0; + gps_odom.pose.covariance[12] = 0; + gps_odom.pose.covariance[13] = 0; + gps_odom.pose.covariance[14] = pow(m_position_accuracy,2); + + m_gps_odom_publisher.publish(gps_odom); + + } catch(tf2::TransformException &e) { + ROS_WARN_STREAM_THROTTLE(10, std::string("Can't get the tf from world to odom") + e.what()); + } + } + } + else + { + if(m_datum_set) + { + if(m_gps.position_covariance[0]-1) + { + f_set_tf(); + } + else{ + ROS_INFO("GPS fix covariance is not good"); + return; + } + } + } +} + + + +bool WorldOdomTransform::f_set_tf() +{ + //check the quality of the gps if the x and y variance is good enough? + + geographic_msgs::GeoPoint ll_point; + geometry_msgs::Point map_point; + nav_msgs::Odometry m_odom_gps_temp = m_odom_gps; + sensor_msgs::NavSatFix m_gps_temp = m_gps; + GeographicLib::MagneticModel magModel("WMM2020"); + + + ll_point.latitude = m_gps_temp.latitude; + ll_point.longitude = m_gps_temp.longitude; + ll_point.altitude = m_gps_temp.altitude; + //Get x and y from lattiude and longitude. x->east, y->north + f_ll2dis(ll_point, map_point); + //get mag declination + double h, Bx, By, Bz; + double H, F, D, I; + magModel(2024, ll_point.latitude,ll_point.longitude, h, Bx, By, Bz); + GeographicLib::MagneticModel::FieldComponents(Bx, By, Bz, H, F, D, I); + //D is negative to west but we are in ENU frame. + if (m_mag_declination_auto) + { + m_mag_declination = -D *M_PI/180.0; + } + // Print the magnetic field components + // printf("declination = %lf\r\n", D); + // printf("tf update\r\n"); + transformStamped.header.stamp = ros::Time::now(); + transformStamped.header.frame_id = m_world_frame; + transformStamped.child_frame_id = m_odom_frame; + + //convert odom xy into world first using magnetic declination + double odom_world_x = m_odom_gps_temp.pose.pose.position.x*cos(m_mag_declination) - m_odom_gps_temp.pose.pose.position.y*sin(m_mag_declination); + double odom_world_y = m_odom_gps_temp.pose.pose.position.x*sin(m_mag_declination) + m_odom_gps_temp.pose.pose.position.y*cos(m_mag_declination); + + double dx = map_point.x -odom_world_x; + double dy = map_point.y - odom_world_y; + printf("gps fix in world =%lf, %lf\r\n", map_point.x, map_point.y); + printf("matching odom position= %lf, %lf\r\n", m_odom_gps_temp.pose.pose.position.x, m_odom_gps_temp.pose.pose.position.y); + + transformStamped.transform.translation.x = dx; + transformStamped.transform.translation.y = dy; + transformStamped.transform.translation.z = m_odom_gps_temp.pose.pose.position.z + 0.0; + + tf2::Quaternion q; + q.setRPY(0, 0, m_mag_declination); + + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + transformStamped.header.stamp = ros::Time::now(); + br.sendTransform(transformStamped); + // sleep(0.1); + + // //checking the m_gps in odom should align with the m_odom_gps + // try { + // auto tf_w2o = m_transform_buffer.lookupTransform( + // m_odom_frame, + // m_world_frame, + // ros::Time(0), + // ros::Duration(1) + // ); + + // auto tf_eigen = tf2::transformToEigen(tf_w2o); + // Eigen::Vector3d p_odom; + + // p_odom = tf_eigen.rotation() * Eigen::Vector3d(map_point.x, map_point.y, 0) + tf_eigen.translation(); + // printf("gps fix points in odom with new tf=%lf,%lf\r\n\r\n", p_odom.x(), p_odom.y()); + + + // } catch(tf2::TransformException &e) { + // ROS_WARN_STREAM_THROTTLE(10, std::string("Can't get the tf from world to odom") + e.what()); + // } + + ROS_INFO("TF Between world and odom is set\r\n"); + m_tf_set = true; + +} + + +void WorldOdomTransform::f_cb_odom(const nav_msgs::Odometry& msg) +{ + m_odom = msg; + //if tf is set i will keep setting the tf + if(m_tf_set) + { + + //publishtf + transformStamped.header.stamp = ros::Time::now(); + br.sendTransform(transformStamped); + //convert odom from odom to world + try { + auto tf_o2w = m_transform_buffer.lookupTransform( + m_world_frame, + m_odom_frame, + ros::Time(0) + ); + geometry_msgs::PoseStamped odom_pose, world_pose; + odom_pose.header = msg.header; + odom_pose.pose = msg.pose.pose; + + // Transform the pose from odom frame to world frame + tf2::doTransform(odom_pose, world_pose, tf_o2w); + geometry_msgs::Point map_point; + geographic_msgs::GeoPoint ll_point; + + map_point.x = world_pose.pose.position.x; + map_point.y = world_pose.pose.position.y; + map_point.z = world_pose.pose.position.z; + + f_dis2ll(map_point, ll_point); + geographic_msgs::GeoPoseStamped geopose; + + geopose.pose.position.latitude = ll_point.latitude; + geopose.pose.position.longitude = ll_point.longitude; + geopose.pose.position.altitude = ll_point.altitude; + geopose.pose.orientation.x = world_pose.pose.orientation.x; + geopose.pose.orientation.y = world_pose.pose.orientation.y; + geopose.pose.orientation.z = world_pose.pose.orientation.z; + geopose.pose.orientation.w = world_pose.pose.orientation.w; + geopose.header= world_pose.header; + + m_geopose_publisher.publish(geopose); + + } catch(tf2::TransformException &e) { + ROS_WARN_STREAM_THROTTLE(10, std::string("Can't get the tf from world to odom") + e.what()); + } + } +} + +bool WorldOdomTransform::f_cb_reset_datum_srv(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &resp) +{ + m_datum_set = false; + m_tf_set = false; + //if the gps covariance is not small we will wait. + ros::Time datum_reset_request_time = ros::Time::now(); + while(ros::Time::now().toSec()-datum_reset_request_time.toSec()-1) + { + m_datum.latitude = m_gps_for_datum.latitude; + m_datum.longitude = m_gps_for_datum.longitude; + m_datum.altitude = m_gps_for_datum.altitude; + m_datum_set = true; + break; + } + sleep(1); + } + if(m_datum_set){ + resp.success = true; + resp.message = "Datum reset done"; + } + else{ + resp.success = false; + resp.message = "Datum failed due to lack of good gps fix within the defined time"; + } +} + +bool WorldOdomTransform::f_cb_reset_tf_srv(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &resp) +{ + m_tf_set = false; + resp.success = true; + resp.message = "tf reset triggered"; +} + +bool WorldOdomTransform::f_cb_fromLL_srv(robot_localization::FromLL::Request &req, robot_localization::FromLL::Response &resp) +{ + f_ll2dis(req.ll_point, resp.map_point); + return true; + +} + +bool WorldOdomTransform::f_cb_toLL_srv(robot_localization::ToLL::Request &req, robot_localization::ToLL::Response &resp) +{ + f_dis2ll(req.map_point, resp.ll_point); + return true; +} + +void WorldOdomTransform::f_ll2dis(geographic_msgs::GeoPoint ll_point, geometry_msgs::Point& map_point) +{ + // double north = m_earthR*(ll_point.latitude - m_datum.latitude)/180.0*M_PI; + // double east = m_earthR*cos(m_datum.latitude/180.0*M_PI) * (ll_point.longitude - m_datum.longitude)/180.0*M_PI; + + //in world frame. + //Geographic lib implementation + double distance, azimuth1, azimuth2; + const GeographicLib::Geodesic& geod = GeographicLib::Geodesic::WGS84(); + geod.Inverse(m_datum.latitude, m_datum.longitude, + ll_point.latitude, ll_point.longitude, + distance, azimuth1, azimuth2); + double north = distance *cos(azimuth1/180.0*M_PI); + double east = distance * sin(azimuth1/180.0*M_PI); + + map_point.x = east; + map_point.y = north; + map_point.z = ll_point.altitude; +} + +void WorldOdomTransform::f_dis2ll(geometry_msgs::Point map_point, geographic_msgs::GeoPoint& ll_point) +{ + //from world frame + // double lat = m_datum.latitude + map_point.y/m_earthR * 180.0/M_PI; + // double lon = m_datum.longitude + map_point.x/(m_earthR*cos(m_datum.latitude/180.0*M_PI)) * 180.0/M_PI; + const GeographicLib::Geodesic& geod = GeographicLib::Geodesic::WGS84(); + double lat, lon; + // Calculate latitude and longitude of target point + geod.Direct(m_datum.latitude, m_datum.longitude, 0.0, map_point.y, lat, lon); // Move north + geod.Direct(lat, lon, 90.0, map_point.x, lat, lon); // Move east + + ll_point.latitude = lat; + ll_point.longitude = lon; + ll_point.altitude = map_point.z; + + +} + + +int main(int argc, char* argv[]) { + + ros::init(argc, argv, "gps_transform"); + + WorldOdomTransform d; + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/alpha_localization/src/gps_odom_transform.hpp b/alpha_localization/src/world_odom_transform.hpp similarity index 76% rename from alpha_localization/src/gps_odom_transform.hpp rename to alpha_localization/src/world_odom_transform.hpp index ab674df..42783d2 100644 --- a/alpha_localization/src/gps_odom_transform.hpp +++ b/alpha_localization/src/world_odom_transform.hpp @@ -25,14 +25,18 @@ #include "ros/ros.h" #include "geometry_msgs/PoseWithCovarianceStamped.h" +#include "geometry_msgs/PoseStamped.h" + #include "mvp_msgs/Float64Stamped.h" #include "geographic_msgs/GeoPoint.h" +#include "geographic_msgs/GeoPoseStamped.h" #include "geometry_msgs/Point.h" #include "sensor_msgs/NavSatFix.h" #include "std_srvs/Trigger.h" #include "robot_localization/FromLL.h" #include "robot_localization/ToLL.h" #include "nav_msgs/Odometry.h" +#include #include #include #include @@ -40,7 +44,8 @@ #include #include "tf2/LinearMath/Matrix3x3.h" #include "tf2_eigen/tf2_eigen.h" - +#include +// #include #include "memory" #include "vector" @@ -48,7 +53,7 @@ #include "functional" #include "cmath" -class GpsOdomTransform{ +class WorldOdomTransform{ private: @@ -58,8 +63,11 @@ class GpsOdomTransform{ ros::Publisher m_gps_odom_publisher; + ros::Publisher m_datum_publisher; + ros::Publisher m_geopose_publisher; + ros::Subscriber m_gps_fix_subscriber; @@ -71,9 +79,15 @@ class GpsOdomTransform{ ros::ServiceServer reset_tf_server; + ros::ServiceServer reset_datum_server; + geographic_msgs::GeoPoint m_datum; - nav_msgs::Odometry m_odom; + nav_msgs::Odometry m_odom, m_odom_gps; + + sensor_msgs::NavSatFix m_gps; + + sensor_msgs::NavSatFix m_gps_for_datum; std::string m_world_frame; @@ -81,6 +95,12 @@ class GpsOdomTransform{ std::string m_tf_prefix; + bool m_datum_set = false; + + bool m_tf_set = false; + + bool m_mag_declination_auto; + double m_earthR = 6371000; double m_mag_declination; @@ -97,9 +117,13 @@ class GpsOdomTransform{ double m_position_accuracy; + bool m_publish_tf; + void f_cb_gps_fix(const sensor_msgs::NavSatFix& msg); - void f_cb_odom(const nav_msgs::OdometryConstPtr& msg); + void f_cb_odom(const nav_msgs::Odometry& msg); + + bool f_cb_reset_datum_srv(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &resp); bool f_cb_reset_tf_srv(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &resp); @@ -110,16 +134,24 @@ class GpsOdomTransform{ void f_ll2dis(geographic_msgs::GeoPoint ll_point, geometry_msgs::Point& map_point); void f_dis2ll(geometry_msgs::Point map_point, geographic_msgs::GeoPoint& ll_point); - + + bool f_set_tf(); + + geometry_msgs::TransformStamped transformStamped; + tf2_ros::StaticTransformBroadcaster br; tf2_ros::Buffer m_transform_buffer; std::shared_ptr m_transform_listener; + public: - GpsOdomTransform(); - void f_check_tf(); + WorldOdomTransform(); + + // void f_check_tf(); + + };