From 018a594b055fae8c9cb952b9c63d78b1451a822e Mon Sep 17 00:00:00 2001 From: mingxi Date: Sat, 22 Jun 2024 22:50:39 -0400 Subject: [PATCH 01/15] added tf broadcast between odom and world --- alpha_localization/src/gps_odom_transform.cpp | 56 ++++++++++++++++--- alpha_localization/src/gps_odom_transform.hpp | 15 ++++- 2 files changed, 59 insertions(+), 12 deletions(-) diff --git a/alpha_localization/src/gps_odom_transform.cpp b/alpha_localization/src/gps_odom_transform.cpp index ca2db62..37f37e7 100644 --- a/alpha_localization/src/gps_odom_transform.cpp +++ b/alpha_localization/src/gps_odom_transform.cpp @@ -22,6 +22,7 @@ */ #include "gps_odom_transform.hpp" +#include GpsOdomTransform::GpsOdomTransform(){ m_nh.reset(new ros::NodeHandle("")); @@ -33,7 +34,7 @@ GpsOdomTransform::GpsOdomTransform(){ m_pnh->param("tf_prefix", m_tf_prefix, ""); - // m_pnh->param("mag_declination", m_mag_declination, 0.0); + 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); @@ -48,6 +49,8 @@ GpsOdomTransform::GpsOdomTransform(){ 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; @@ -92,7 +95,7 @@ GpsOdomTransform::GpsOdomTransform(){ std_srvs::Trigger::Response> ( "reset_datum", - std::bind(&GpsOdomTransform::f_cb_reset_tf_srv, + std::bind(&GpsOdomTransform::f_cb_reset_datum_srv, this,std::placeholders::_1,std::placeholders::_2 ) ); @@ -115,14 +118,16 @@ void GpsOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) ll_point.altitude = msg.altitude; //Get x and y from lattiude and longitude. x->east, y->north f_ll2dis(ll_point, map_point); - + if(m_publish_tf) + { + f_update_tf(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) + ros::Time(0) ); Eigen::Vector3d p_world; auto tf_eigen = tf2::transformToEigen(tf_w2o); @@ -149,20 +154,43 @@ void GpsOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) gps_world.pose.covariance[14] = pow(m_position_accuracy,2); - m_gps_odom_publisher.publish(gps_world); - m_datum_publisher.publish(m_datum); + 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_update_tf(geometry_msgs::Point map_point) +{ + + // 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; + transformStamped.transform.translation.x = m_odom.pose.pose.position.x - map_point.x; + transformStamped.transform.translation.y = m_odom.pose.pose.position.y - map_point.y; + transformStamped.transform.translation.z = m_odom.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(); + br.sendTransform(transformStamped); } + void GpsOdomTransform::f_cb_odom(const nav_msgs::OdometryConstPtr& msg) { - // m_odom = *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_reset_datum_srv(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &resp) { } @@ -209,7 +237,17 @@ int main(int argc, char* argv[]) { GpsOdomTransform d; ros::spin(); + // ros::Rate loop_rate(10); + // while (ros::ok()) + // { + + // ros::spinOnce(); + // printf("publishing tf, %s->%s\r\n", d.transformStamped.header.frame_id.c_str(), d.transformStamped.child_frame_id.c_str()); + // d.br.sendTransform(d.transformStamped); + + // loop_rate.sleep(); + // } return 0; } \ No newline at end of file diff --git a/alpha_localization/src/gps_odom_transform.hpp b/alpha_localization/src/gps_odom_transform.hpp index ab674df..6c862ec 100644 --- a/alpha_localization/src/gps_odom_transform.hpp +++ b/alpha_localization/src/gps_odom_transform.hpp @@ -40,7 +40,7 @@ #include #include "tf2/LinearMath/Matrix3x3.h" #include "tf2_eigen/tf2_eigen.h" - +#include #include "memory" #include "vector" @@ -81,6 +81,8 @@ class GpsOdomTransform{ std::string m_tf_prefix; + + double m_earthR = 6371000; double m_mag_declination; @@ -97,11 +99,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); - bool f_cb_reset_tf_srv(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &resp); + bool f_cb_reset_datum_srv(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &resp); bool f_cb_fromLL_srv(robot_localization::FromLL::Request &req, robot_localization::FromLL::Response &resp); @@ -110,6 +114,8 @@ 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); + + void f_update_tf(geometry_msgs::Point map_point); tf2_ros::Buffer m_transform_buffer; @@ -119,7 +125,10 @@ class GpsOdomTransform{ public: GpsOdomTransform(); - void f_check_tf(); + // void f_check_tf(); + + geometry_msgs::TransformStamped transformStamped; + tf2_ros::TransformBroadcaster br; }; From 77f2ec61167dfdca5218e98ec0efa0ca0c875c3c Mon Sep 17 00:00:00 2001 From: mingxi Date: Sat, 22 Jun 2024 22:54:53 -0400 Subject: [PATCH 02/15] testing --- alpha_localization/src/gps_odom_transform.cpp | 3 --- alpha_localization/src/gps_odom_transform.hpp | 2 -- 2 files changed, 5 deletions(-) diff --git a/alpha_localization/src/gps_odom_transform.cpp b/alpha_localization/src/gps_odom_transform.cpp index 37f37e7..e276827 100644 --- a/alpha_localization/src/gps_odom_transform.cpp +++ b/alpha_localization/src/gps_odom_transform.cpp @@ -139,8 +139,6 @@ void GpsOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) + 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); @@ -153,7 +151,6 @@ void GpsOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) 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); diff --git a/alpha_localization/src/gps_odom_transform.hpp b/alpha_localization/src/gps_odom_transform.hpp index 6c862ec..5e49fd5 100644 --- a/alpha_localization/src/gps_odom_transform.hpp +++ b/alpha_localization/src/gps_odom_transform.hpp @@ -81,8 +81,6 @@ class GpsOdomTransform{ std::string m_tf_prefix; - - double m_earthR = 6371000; double m_mag_declination; From cab2d4487d283569a0687d577fff300c8632aa50 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 09:24:32 -0400 Subject: [PATCH 03/15] added tf set function and check --- alpha_localization/src/gps_odom_transform.cpp | 125 ++++++++++++------ alpha_localization/src/gps_odom_transform.hpp | 21 ++- 2 files changed, 96 insertions(+), 50 deletions(-) diff --git a/alpha_localization/src/gps_odom_transform.cpp b/alpha_localization/src/gps_odom_transform.cpp index e276827..9e37da9 100644 --- a/alpha_localization/src/gps_odom_transform.cpp +++ b/alpha_localization/src/gps_odom_transform.cpp @@ -43,9 +43,9 @@ GpsOdomTransform::GpsOdomTransform(){ 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_latitude", m_datum_latitude, 0.0); - m_pnh->param("datum_longitude", m_datum_longitude, -71.0); + m_pnh->param("datum_longitude", m_datum_longitude, 0.0); m_pnh->param("datum_altitude", m_datum_altitude, 0.0); @@ -55,6 +55,12 @@ GpsOdomTransform::GpsOdomTransform(){ 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; + } + m_tf_set = false; m_world_frame = m_tf_prefix + "/" + m_world_frame; m_odom_frame = m_tf_prefix + "/" + m_odom_frame; @@ -113,32 +119,41 @@ void GpsOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) 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); - if(m_publish_tf) + m_gps = msg; + m_odom_gps = m_odom; //map the most recent odom; + + + if(m_datum_set) { - f_update_tf(map_point); + m_datum_publisher.publish(m_datum); } - //convert distance from gps into odom frame using mag_declination. - try { + // printf("Got GPS\r\n"); + //only do the following if the tf between world and odom are set. + if(m_tf_set) + { + 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) ); - 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(); + Eigen::Vector3d p_odom; + p_odom = tf_eigen.rotation() * Eigen::Vector3d(map_point.x, map_point.y, map_point.z) + tf_eigen.translation(); + // printf("Latlon=%lf, %lf\r\n", ll_point.latitude, ll_point.longitude); + // printf("T_matrix=%lf,%lf\r\n", tf_eigen.translation().x(), tf_eigen.translation().y()); + // printf("map_point=%lf,%lf\r\n", map_point.x, map_point.y); + // printf("translated=%lf, %lf\r\n", p_odom.x(), p_odom.y()); + // printf("odom_point=%lf,%lf\r\n",m_odom.pose.pose.position.x, m_odom.pose.pose.position.y); + gps_world.pose.pose.position.x = p_odom.x(); + gps_world.pose.pose.position.y = p_odom.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); @@ -151,40 +166,73 @@ void GpsOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) 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); - - + m_gps_odom_publisher.publish(gps_world); } 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_gps.position_covariance[0]east, y->north + f_ll2dis(ll_point, map_point); + // 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; - transformStamped.transform.translation.x = m_odom.pose.pose.position.x - map_point.x; - transformStamped.transform.translation.y = m_odom.pose.pose.position.y - map_point.y; - transformStamped.transform.translation.z = m_odom.pose.pose.position.z -0.0; + double dx = -m_odom_gps.pose.pose.position.x + map_point.x; + double dy = -m_odom_gps.pose.pose.position.y + map_point.y; + // printf("dx=%lf, dy =%lf\r\n", dx, dy); + transformStamped.transform.translation.x = dx; + transformStamped.transform.translation.y = dy; + transformStamped.transform.translation.z = m_odom_gps.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); + + ROS_INFO("TF Between world and odom is set\r\n"); + m_tf_set = true; + } -void GpsOdomTransform::f_cb_odom(const nav_msgs::OdometryConstPtr& msg) +void GpsOdomTransform::f_cb_odom(const nav_msgs::Odometry& msg) { - m_odom = *msg; - + // printf("got odometry\r\n"); + + m_odom = msg; + //if tf is set i will keep setting the tf + if(m_tf_set) + { + transformStamped.header.stamp = ros::Time::now(); + br.sendTransform(transformStamped); + } } bool GpsOdomTransform::f_cb_reset_datum_srv(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &resp) @@ -232,19 +280,8 @@ int main(int argc, char* argv[]) { ros::init(argc, argv, "gps_transform"); GpsOdomTransform d; - + ros::spin(); - // ros::Rate loop_rate(10); - // while (ros::ok()) - // { - - // ros::spinOnce(); - // printf("publishing tf, %s->%s\r\n", d.transformStamped.header.frame_id.c_str(), d.transformStamped.child_frame_id.c_str()); - // d.br.sendTransform(d.transformStamped); - - // loop_rate.sleep(); - - // } return 0; } \ No newline at end of file diff --git a/alpha_localization/src/gps_odom_transform.hpp b/alpha_localization/src/gps_odom_transform.hpp index 5e49fd5..a6ade6e 100644 --- a/alpha_localization/src/gps_odom_transform.hpp +++ b/alpha_localization/src/gps_odom_transform.hpp @@ -73,7 +73,9 @@ class GpsOdomTransform{ geographic_msgs::GeoPoint m_datum; - nav_msgs::Odometry m_odom; + nav_msgs::Odometry m_odom, m_odom_gps; + + sensor_msgs::NavSatFix m_gps; std::string m_world_frame; @@ -81,6 +83,10 @@ class GpsOdomTransform{ std::string m_tf_prefix; + bool m_datum_set = false; + + bool m_tf_set = false; + double m_earthR = 6371000; double m_mag_declination; @@ -101,7 +107,7 @@ class GpsOdomTransform{ 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); @@ -113,20 +119,23 @@ class GpsOdomTransform{ void f_dis2ll(geometry_msgs::Point map_point, geographic_msgs::GeoPoint& ll_point); - void f_update_tf(geometry_msgs::Point map_point); - + bool f_set_tf(); + + geometry_msgs::TransformStamped transformStamped; + tf2_ros::TransformBroadcaster br; tf2_ros::Buffer m_transform_buffer; std::shared_ptr m_transform_listener; + public: GpsOdomTransform(); + // void f_check_tf(); - geometry_msgs::TransformStamped transformStamped; - tf2_ros::TransformBroadcaster br; + }; From 5adf43639e65a8ab51dacf7789cc9709d8339794 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 09:33:48 -0400 Subject: [PATCH 04/15] changed name for depth inclusion --- alpha_localization/CMakeLists.txt | 6 ++-- ...transform.cpp => world_odom_transform.cpp} | 34 ++++++++++--------- ...transform.hpp => world_odom_transform.hpp} | 4 +-- 3 files changed, 23 insertions(+), 21 deletions(-) rename alpha_localization/src/{gps_odom_transform.cpp => world_odom_transform.cpp} (87%) rename alpha_localization/src/{gps_odom_transform.hpp => world_odom_transform.hpp} (98%) diff --git a/alpha_localization/CMakeLists.txt b/alpha_localization/CMakeLists.txt index 63f315e..da1299b 100644 --- a/alpha_localization/CMakeLists.txt +++ b/alpha_localization/CMakeLists.txt @@ -54,7 +54,7 @@ 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 @@ -68,7 +68,7 @@ add_dependencies(depth_filter_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_ 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 @@ -87,6 +87,6 @@ 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/src/gps_odom_transform.cpp b/alpha_localization/src/world_odom_transform.cpp similarity index 87% rename from alpha_localization/src/gps_odom_transform.cpp rename to alpha_localization/src/world_odom_transform.cpp index 9e37da9..343134a 100644 --- a/alpha_localization/src/gps_odom_transform.cpp +++ b/alpha_localization/src/world_odom_transform.cpp @@ -21,10 +21,10 @@ Copyright (C) 2024 Smart Ocean Systems Laboratory */ -#include "gps_odom_transform.hpp" +#include "world_odom_transform.hpp" #include -GpsOdomTransform::GpsOdomTransform(){ +WorldOdomTransform::WorldOdomTransform(){ m_nh.reset(new ros::NodeHandle("")); m_pnh.reset(new ros::NodeHandle("~")); @@ -71,9 +71,9 @@ GpsOdomTransform::GpsOdomTransform(){ m_gps_fix_subscriber = m_nh->subscribe("gps/fix", 10, - &GpsOdomTransform::f_cb_gps_fix, this); + &WorldOdomTransform::f_cb_gps_fix, this); m_odom_subscriber = m_nh->subscribe("odometry", 10, - &GpsOdomTransform::f_cb_odom, this); + &WorldOdomTransform::f_cb_odom, this); /** * Initialize services @@ -82,7 +82,7 @@ GpsOdomTransform::GpsOdomTransform(){ robot_localization::FromLL::Response> ( "fromLL", - std::bind(&GpsOdomTransform::f_cb_fromLL_srv, + std::bind(&WorldOdomTransform::f_cb_fromLL_srv, this,std::placeholders::_1,std::placeholders::_2 ) ); @@ -91,7 +91,7 @@ GpsOdomTransform::GpsOdomTransform(){ robot_localization::ToLL::Response> ( "toLL", - std::bind(&GpsOdomTransform::f_cb_toLL_srv, + std::bind(&WorldOdomTransform::f_cb_toLL_srv, this, std::placeholders::_1,std::placeholders::_2 ) @@ -101,7 +101,7 @@ GpsOdomTransform::GpsOdomTransform(){ std_srvs::Trigger::Response> ( "reset_datum", - std::bind(&GpsOdomTransform::f_cb_reset_datum_srv, + std::bind(&WorldOdomTransform::f_cb_reset_datum_srv, this,std::placeholders::_1,std::placeholders::_2 ) ); @@ -112,7 +112,7 @@ GpsOdomTransform::GpsOdomTransform(){ } -void GpsOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) +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; @@ -126,6 +126,7 @@ void GpsOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) if(m_datum_set) { m_datum_publisher.publish(m_datum); + return; } // printf("Got GPS\r\n"); //only do the following if the tf between world and odom are set. @@ -179,11 +180,12 @@ void GpsOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) } else{ ROS_INFO("GPS fix covariance is more than %lf\r\n", m_acceptable_var); + return; } } } -bool GpsOdomTransform::f_set_tf() +bool WorldOdomTransform::f_set_tf() { //check the quality of the gps if the x and y variance is good enough? @@ -222,7 +224,7 @@ bool GpsOdomTransform::f_set_tf() } -void GpsOdomTransform::f_cb_odom(const nav_msgs::Odometry& msg) +void WorldOdomTransform::f_cb_odom(const nav_msgs::Odometry& msg) { // printf("got odometry\r\n"); @@ -235,25 +237,25 @@ void GpsOdomTransform::f_cb_odom(const nav_msgs::Odometry& msg) } } -bool GpsOdomTransform::f_cb_reset_datum_srv(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &resp) +bool WorldOdomTransform::f_cb_reset_datum_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) +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 GpsOdomTransform::f_cb_toLL_srv(robot_localization::ToLL::Request &req, robot_localization::ToLL::Response &resp) +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 GpsOdomTransform::f_ll2dis(geographic_msgs::GeoPoint ll_point, geometry_msgs::Point& map_point) +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; @@ -264,7 +266,7 @@ void GpsOdomTransform::f_ll2dis(geographic_msgs::GeoPoint ll_point, geometry_msg } -void GpsOdomTransform::f_dis2ll(geometry_msgs::Point map_point, geographic_msgs::GeoPoint& ll_point) +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; @@ -279,7 +281,7 @@ int main(int argc, char* argv[]) { ros::init(argc, argv, "gps_transform"); - GpsOdomTransform d; + WorldOdomTransform d; ros::spin(); diff --git a/alpha_localization/src/gps_odom_transform.hpp b/alpha_localization/src/world_odom_transform.hpp similarity index 98% rename from alpha_localization/src/gps_odom_transform.hpp rename to alpha_localization/src/world_odom_transform.hpp index a6ade6e..0c2af52 100644 --- a/alpha_localization/src/gps_odom_transform.hpp +++ b/alpha_localization/src/world_odom_transform.hpp @@ -48,7 +48,7 @@ #include "functional" #include "cmath" -class GpsOdomTransform{ +class WorldOdomTransform{ private: @@ -131,7 +131,7 @@ class GpsOdomTransform{ public: - GpsOdomTransform(); + WorldOdomTransform(); // void f_check_tf(); From 59a4a5eb532886f2c46b76ac1a2495ebce208d27 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 11:16:04 -0400 Subject: [PATCH 05/15] added geopose publisher into world_odom_transform --- alpha_localization/CMakeLists.txt | 2 + alpha_localization/package.xml | 2 + .../src/world_odom_transform.cpp | 49 +++++++++++++++++-- .../src/world_odom_transform.hpp | 10 +++- 4 files changed, 58 insertions(+), 5 deletions(-) diff --git a/alpha_localization/CMakeLists.txt b/alpha_localization/CMakeLists.txt index da1299b..2ed4f05 100644 --- a/alpha_localization/CMakeLists.txt +++ b/alpha_localization/CMakeLists.txt @@ -14,6 +14,8 @@ find_package(catkin REQUIRED COMPONENTS geographic_msgs nav_msgs tf2_eigen + tf2 + tf2_ros ) find_package(Eigen3 REQUIRED) 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/src/world_odom_transform.cpp b/alpha_localization/src/world_odom_transform.cpp index 343134a..68ade0a 100644 --- a/alpha_localization/src/world_odom_transform.cpp +++ b/alpha_localization/src/world_odom_transform.cpp @@ -69,6 +69,8 @@ WorldOdomTransform::WorldOdomTransform(){ 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); @@ -227,13 +229,52 @@ bool WorldOdomTransform::f_set_tf() void WorldOdomTransform::f_cb_odom(const nav_msgs::Odometry& msg) { // printf("got odometry\r\n"); - m_odom = msg; + // printf("tf_set =%d \r\n", m_tf_set); //if tf is set i will keep setting the tf if(m_tf_set) - { - transformStamped.header.stamp = ros::Time::now(); - br.sendTransform(transformStamped); + { + + //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()); + } } } diff --git a/alpha_localization/src/world_odom_transform.hpp b/alpha_localization/src/world_odom_transform.hpp index 0c2af52..64ff61f 100644 --- a/alpha_localization/src/world_odom_transform.hpp +++ b/alpha_localization/src/world_odom_transform.hpp @@ -25,15 +25,19 @@ #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 #include #include @@ -41,6 +45,7 @@ #include "tf2/LinearMath/Matrix3x3.h" #include "tf2_eigen/tf2_eigen.h" #include +// #include #include "memory" #include "vector" @@ -58,8 +63,11 @@ class WorldOdomTransform{ ros::Publisher m_gps_odom_publisher; + ros::Publisher m_datum_publisher; + ros::Publisher m_geopose_publisher; + ros::Subscriber m_gps_fix_subscriber; From 4cfbbcd57b12e51b3c2a2af292d2e14c805295eb Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 13:17:56 -0400 Subject: [PATCH 06/15] use doTransform function for pose conversion --- .../src/world_odom_transform.cpp | 35 +++++++++++++------ 1 file changed, 24 insertions(+), 11 deletions(-) diff --git a/alpha_localization/src/world_odom_transform.cpp b/alpha_localization/src/world_odom_transform.cpp index 68ade0a..c0d7c47 100644 --- a/alpha_localization/src/world_odom_transform.cpp +++ b/alpha_localization/src/world_odom_transform.cpp @@ -146,17 +146,30 @@ void WorldOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) m_world_frame, ros::Time(0) ); - auto tf_eigen = tf2::transformToEigen(tf_w2o); - - Eigen::Vector3d p_odom; - p_odom = tf_eigen.rotation() * Eigen::Vector3d(map_point.x, map_point.y, map_point.z) + tf_eigen.translation(); - // printf("Latlon=%lf, %lf\r\n", ll_point.latitude, ll_point.longitude); - // printf("T_matrix=%lf,%lf\r\n", tf_eigen.translation().x(), tf_eigen.translation().y()); - // printf("map_point=%lf,%lf\r\n", map_point.x, map_point.y); - // printf("translated=%lf, %lf\r\n", p_odom.x(), p_odom.y()); - // printf("odom_point=%lf,%lf\r\n",m_odom.pose.pose.position.x, m_odom.pose.pose.position.y); - gps_world.pose.pose.position.x = p_odom.x(); - gps_world.pose.pose.position.y = p_odom.y(); + + geometry_msgs::PoseStamped odom_pose, world_pose; + world_pose.header = msg.header; + 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 = 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); + + // Eigen::Vector3d p_odom; + // p_odom = tf_eigen.rotation() * Eigen::Vector3d(map_point.x, map_point.y, map_point.z) + tf_eigen.translation(); + // // printf("Latlon=%lf, %lf\r\n", ll_point.latitude, ll_point.longitude); + // // printf("T_matrix=%lf,%lf\r\n", tf_eigen.translation().x(), tf_eigen.translation().y()); + // // printf("map_point=%lf,%lf\r\n", map_point.x, map_point.y); + // // printf("translated=%lf, %lf\r\n", p_odom.x(), p_odom.y()); + // // printf("odom_point=%lf,%lf\r\n",m_odom.pose.pose.position.x, m_odom.pose.pose.position.y); + gps_world.pose.pose.position.x = odom_pose.pose.position.x; + gps_world.pose.pose.position.y = odom_pose.pose.position.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); From a834823e4b8253e423214b3fa5238cb128898c14 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 19:45:23 -0400 Subject: [PATCH 07/15] fixed tf broadcast bug --- .../src/world_odom_transform.cpp | 168 +++++++++++++----- .../src/world_odom_transform.hpp | 10 +- 2 files changed, 129 insertions(+), 49 deletions(-) diff --git a/alpha_localization/src/world_odom_transform.cpp b/alpha_localization/src/world_odom_transform.cpp index c0d7c47..e0c64a7 100644 --- a/alpha_localization/src/world_odom_transform.cpp +++ b/alpha_localization/src/world_odom_transform.cpp @@ -55,11 +55,14 @@ WorldOdomTransform::WorldOdomTransform(){ m_datum.longitude = m_datum_longitude; m_datum.altitude = m_datum_altitude; - if(m_datum.latitude==0) - { + + 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; @@ -99,7 +102,7 @@ WorldOdomTransform::WorldOdomTransform(){ ) ); - reset_tf_server = m_pnh->advertiseServiceadvertiseService ( "reset_datum", @@ -108,6 +111,15 @@ WorldOdomTransform::WorldOdomTransform(){ ) ); + 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) ); @@ -118,25 +130,27 @@ 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_world; + 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); - return; } // printf("Got GPS\r\n"); //only do the following if the tf between world and odom are set. - if(m_tf_set) + if(m_tf_set && m_datum_set) { + 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. @@ -149,67 +163,69 @@ void WorldOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) 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 = 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); - - // Eigen::Vector3d p_odom; - // p_odom = tf_eigen.rotation() * Eigen::Vector3d(map_point.x, map_point.y, map_point.z) + tf_eigen.translation(); - // // printf("Latlon=%lf, %lf\r\n", ll_point.latitude, ll_point.longitude); - // // printf("T_matrix=%lf,%lf\r\n", tf_eigen.translation().x(), tf_eigen.translation().y()); - // // printf("map_point=%lf,%lf\r\n", map_point.x, map_point.y); - // // printf("translated=%lf, %lf\r\n", p_odom.x(), p_odom.y()); - // // printf("odom_point=%lf,%lf\r\n",m_odom.pose.pose.position.x, m_odom.pose.pose.position.y); - gps_world.pose.pose.position.x = odom_pose.pose.position.x; - gps_world.pose.pose.position.y = odom_pose.pose.position.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); + 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_gps.position_covariance[0]east, y->north f_ll2dis(ll_point, map_point); @@ -217,21 +233,50 @@ bool WorldOdomTransform::f_set_tf() transformStamped.header.stamp = ros::Time::now(); transformStamped.header.frame_id = m_world_frame; transformStamped.child_frame_id = m_odom_frame; - double dx = -m_odom_gps.pose.pose.position.x + map_point.x; - double dy = -m_odom_gps.pose.pose.position.y + map_point.y; - // printf("dx=%lf, dy =%lf\r\n", dx, dy); + + //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.pose.pose.position.z - 0.0; + 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; @@ -241,9 +286,7 @@ bool WorldOdomTransform::f_set_tf() void WorldOdomTransform::f_cb_odom(const nav_msgs::Odometry& msg) { - // printf("got odometry\r\n"); m_odom = msg; - // printf("tf_set =%d \r\n", m_tf_set); //if tf is set i will keep setting the tf if(m_tf_set) { @@ -293,7 +336,38 @@ void WorldOdomTransform::f_cb_odom(const nav_msgs::Odometry& msg) 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() -// #include +#include #include #include #include @@ -79,12 +79,16 @@ class WorldOdomTransform{ ros::ServiceServer reset_tf_server; + ros::ServiceServer reset_datum_server; + geographic_msgs::GeoPoint m_datum; 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; std::string m_odom_frame; @@ -119,6 +123,8 @@ class WorldOdomTransform{ 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); + bool f_cb_fromLL_srv(robot_localization::FromLL::Request &req, robot_localization::FromLL::Response &resp); bool f_cb_toLL_srv(robot_localization::ToLL::Request &req, robot_localization::ToLL::Response &resp); @@ -130,7 +136,7 @@ class WorldOdomTransform{ bool f_set_tf(); geometry_msgs::TransformStamped transformStamped; - tf2_ros::TransformBroadcaster br; + tf2_ros::StaticTransformBroadcaster br; tf2_ros::Buffer m_transform_buffer; From 88fa93e5fd403e454bb3991c9b0aa265d79fa8c9 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 21:19:22 -0400 Subject: [PATCH 08/15] added geographic lib to conver latlon and distance, and use the mag model to compute the mag declination automatically --- alpha_localization/CMakeLists.txt | 4 ++ alpha_localization/readme.md | 22 +++++++++ .../src/world_odom_transform.cpp | 46 +++++++++++++++---- 3 files changed, 64 insertions(+), 8 deletions(-) create mode 100644 alpha_localization/readme.md diff --git a/alpha_localization/CMakeLists.txt b/alpha_localization/CMakeLists.txt index 2ed4f05..3b7bc9a 100644 --- a/alpha_localization/CMakeLists.txt +++ b/alpha_localization/CMakeLists.txt @@ -19,6 +19,10 @@ find_package(catkin REQUIRED COMPONENTS ) 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( 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/world_odom_transform.cpp b/alpha_localization/src/world_odom_transform.cpp index e0c64a7..52a8d5f 100644 --- a/alpha_localization/src/world_odom_transform.cpp +++ b/alpha_localization/src/world_odom_transform.cpp @@ -23,6 +23,9 @@ #include "world_odom_transform.hpp" #include +#include +#include + WorldOdomTransform::WorldOdomTransform(){ m_nh.reset(new ros::NodeHandle("")); @@ -34,7 +37,7 @@ WorldOdomTransform::WorldOdomTransform(){ m_pnh->param("tf_prefix", m_tf_prefix, ""); - m_pnh->param("mag_declination", m_mag_declination, 0.0); + // 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); @@ -222,13 +225,23 @@ bool WorldOdomTransform::f_set_tf() 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. + 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; @@ -385,23 +398,40 @@ bool WorldOdomTransform::f_cb_toLL_srv(robot_localization::ToLL::Request &req, r 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; + // 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; - //in world frame. - } 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; + // 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; + + } From 77fe671f4cfc03e127a326a27dd06c63c96252ed Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 21:21:28 -0400 Subject: [PATCH 09/15] added mag declination auto option for simulation or INS with mag delination auto corrected in the data --- alpha_localization/src/world_odom_transform.cpp | 9 +++++++-- alpha_localization/src/world_odom_transform.hpp | 2 ++ 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/alpha_localization/src/world_odom_transform.cpp b/alpha_localization/src/world_odom_transform.cpp index 52a8d5f..d3c46ee 100644 --- a/alpha_localization/src/world_odom_transform.cpp +++ b/alpha_localization/src/world_odom_transform.cpp @@ -37,7 +37,9 @@ WorldOdomTransform::WorldOdomTransform(){ m_pnh->param("tf_prefix", m_tf_prefix, ""); - // m_pnh->param("mag_declination", m_mag_declination, 0.0); + 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); @@ -239,7 +241,10 @@ bool WorldOdomTransform::f_set_tf() 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. - m_mag_declination = -D *M_PI/180.0; + 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"); diff --git a/alpha_localization/src/world_odom_transform.hpp b/alpha_localization/src/world_odom_transform.hpp index 5167e75..42783d2 100644 --- a/alpha_localization/src/world_odom_transform.hpp +++ b/alpha_localization/src/world_odom_transform.hpp @@ -99,6 +99,8 @@ class WorldOdomTransform{ bool m_tf_set = false; + bool m_mag_declination_auto; + double m_earthR = 6371000; double m_mag_declination; From dde15906a0de57faa9d32a327a727488844b09b4 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 21:22:40 -0400 Subject: [PATCH 10/15] removed geopose_publisher --- alpha_localization/CMakeLists.txt | 5 -- .../src/geopose_publisher_node.cpp | 88 ------------------- .../src/geopose_publisher_node.hpp | 57 ------------ 3 files changed, 150 deletions(-) delete mode 100644 alpha_localization/src/geopose_publisher_node.cpp delete mode 100644 alpha_localization/src/geopose_publisher_node.hpp diff --git a/alpha_localization/CMakeLists.txt b/alpha_localization/CMakeLists.txt index 3b7bc9a..597de78 100644 --- a/alpha_localization/CMakeLists.txt +++ b/alpha_localization/CMakeLists.txt @@ -59,7 +59,6 @@ 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(world_odom_transform_node src/world_odom_transform.cpp) ## Rename C++ executable without prefix @@ -73,7 +72,6 @@ add_executable(world_odom_transform_node src/world_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(world_odom_transform_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against @@ -89,9 +87,6 @@ target_link_libraries(imu_ned_to_enu ${catkin_LIBRARIES} ) -target_link_libraries(geopose_publisher_node - ${catkin_LIBRARIES} -) target_link_libraries(world_odom_transform_node ${catkin_LIBRARIES} 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(); - -}; From f886042926594e585df0d9bc2b1252295f1175b6 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 21:24:13 -0400 Subject: [PATCH 11/15] added an example for world_odom_transform --- .../launch/gps_odom_transform.launch | 19 --------------- .../launch/world_odom_transform.launch | 24 +++++++++++++++++++ 2 files changed, 24 insertions(+), 19 deletions(-) delete mode 100644 alpha_localization/launch/gps_odom_transform.launch create mode 100644 alpha_localization/launch/world_odom_transform.launch 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 From 31da245d879fb4dd68da2b28ccd285627b9bdd80 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 21:27:24 -0400 Subject: [PATCH 12/15] changed fromLL toLL service name to be compatible with existing nodes --- alpha_localization/src/world_odom_transform.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/alpha_localization/src/world_odom_transform.cpp b/alpha_localization/src/world_odom_transform.cpp index d3c46ee..69b0ac9 100644 --- a/alpha_localization/src/world_odom_transform.cpp +++ b/alpha_localization/src/world_odom_transform.cpp @@ -88,7 +88,7 @@ WorldOdomTransform::WorldOdomTransform(){ /** * Initialize services */ - fromLL_server = m_pnh->advertiseServiceadvertiseService ( "fromLL", @@ -97,7 +97,7 @@ WorldOdomTransform::WorldOdomTransform(){ ) ); - toLL_server = m_pnh->advertiseServiceadvertiseService ( "toLL", From 0fd8f32a7dd1f13559ffc552894370f6e4f7ead2 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 22:22:41 -0400 Subject: [PATCH 13/15] added gps status check --- alpha_localization/src/world_odom_transform.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/alpha_localization/src/world_odom_transform.cpp b/alpha_localization/src/world_odom_transform.cpp index 69b0ac9..47342b6 100644 --- a/alpha_localization/src/world_odom_transform.cpp +++ b/alpha_localization/src/world_odom_transform.cpp @@ -205,7 +205,9 @@ void WorldOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) { if(m_datum_set) { - if(m_gps.position_covariance[0]-1) { f_set_tf(); } @@ -361,7 +363,9 @@ bool WorldOdomTransform::f_cb_reset_datum_srv(std_srvs::Trigger::Request &req, s 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; From 5ea7afeec2a171dba83337ac053ad31ffbcbe272 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 23:00:01 -0400 Subject: [PATCH 14/15] added gps quality check for gps/odometry publish --- .../src/world_odom_transform.cpp | 102 +++++++++--------- 1 file changed, 53 insertions(+), 49 deletions(-) diff --git a/alpha_localization/src/world_odom_transform.cpp b/alpha_localization/src/world_odom_transform.cpp index 47342b6..770886d 100644 --- a/alpha_localization/src/world_odom_transform.cpp +++ b/alpha_localization/src/world_odom_transform.cpp @@ -151,54 +151,58 @@ void WorldOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) //only do the following if the tf between world and odom are set. if(m_tf_set && m_datum_set) { - - 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()); + 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 @@ -212,7 +216,7 @@ void WorldOdomTransform::f_cb_gps_fix(const sensor_msgs::NavSatFix& msg) f_set_tf(); } else{ - ROS_INFO("GPS fix covariance is more than %lf\r\n", m_acceptable_var); + ROS_INFO("GPS fix covariance is not good"); return; } } From 22d6fc4193fa5e06b899e84f821f03aebc68f947 Mon Sep 17 00:00:00 2001 From: mingxi Date: Sun, 23 Jun 2024 23:18:57 -0400 Subject: [PATCH 15/15] changed the z transform between world and odom --- alpha_localization/src/world_odom_transform.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/alpha_localization/src/world_odom_transform.cpp b/alpha_localization/src/world_odom_transform.cpp index 770886d..f8d2566 100644 --- a/alpha_localization/src/world_odom_transform.cpp +++ b/alpha_localization/src/world_odom_transform.cpp @@ -269,7 +269,7 @@ bool WorldOdomTransform::f_set_tf() transformStamped.transform.translation.x = dx; transformStamped.transform.translation.y = dy; - transformStamped.transform.translation.z = -m_odom_gps_temp.pose.pose.position.z + 0.0; + transformStamped.transform.translation.z = m_odom_gps_temp.pose.pose.position.z + 0.0; tf2::Quaternion q; q.setRPY(0, 0, m_mag_declination);