From a747284c7ca0e3119fd64abf1daa26d41680dd5a Mon Sep 17 00:00:00 2001 From: BillThePlatypus Date: Fri, 6 Sep 2019 14:00:03 -0600 Subject: [PATCH 1/6] Adjust existing GPS handling to use NavSatFix --- rosplane/include/estimator_base.h | 4 ++-- rosplane/src/estimator_base.cpp | 10 ++++++---- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/rosplane/include/estimator_base.h b/rosplane/include/estimator_base.h index 0bcec17..d2821ae 100644 --- a/rosplane/include/estimator_base.h +++ b/rosplane/include/estimator_base.h @@ -11,7 +11,7 @@ #include #include -#include +#include #include #include #include @@ -98,7 +98,7 @@ class estimator_base ros::Subscriber status_sub_; void update(const ros::TimerEvent &); - void gpsCallback(const rosflight_msgs::GPS &msg); + void gpsCallback(const sensor_msgs::NavSatFix &msg); void imuCallback(const sensor_msgs::Imu &msg); void baroAltCallback(const rosflight_msgs::Barometer &msg); void airspeedCallback(const rosflight_msgs::Airspeed &msg); diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_base.cpp index bef5862..e12598a 100644 --- a/rosplane/src/estimator_base.cpp +++ b/rosplane/src/estimator_base.cpp @@ -1,5 +1,6 @@ #include "estimator_base.h" #include "estimator_example.h" +#include namespace rosplane { @@ -8,7 +9,7 @@ estimator_base::estimator_base(): nh_(ros::NodeHandle()), nh_private_(ros::NodeHandle("~")) { - nh_private_.param("gps_topic", gps_topic_, "gps"); + nh_private_.param("gps_topic", gps_topic_, "navsat_fix"); nh_private_.param("imu_topic", imu_topic_, "imu/data"); nh_private_.param("baro_topic", baro_topic_, "baro"); nh_private_.param("airspeed_topic", airspeed_topic_, "airspeed"); @@ -92,9 +93,10 @@ void estimator_base::update(const ros::TimerEvent &) vehicle_state_pub_.publish(msg); } -void estimator_base::gpsCallback(const rosflight_msgs::GPS &msg) +void estimator_base::gpsCallback(const sensor_msgs::NavSatFix &msg) { - if (msg.fix != true || msg.NumSat < 4 || !std::isfinite(msg.latitude)) + bool has_fix = msg.status.status >= sensor_msgs::NavSatStatus::STATUS_FIX; // Higher values refer to augmented fixes + if (!has_fix || !std::isfinite(msg.latitude)) { input_.gps_new = false; return; @@ -112,7 +114,7 @@ void estimator_base::gpsCallback(const rosflight_msgs::GPS &msg) input_.gps_e = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0; input_.gps_h = msg.altitude - init_alt_; input_.gps_Vg = msg.speed; - if (msg.speed > 0.3) + if (msg.speed > 0.3) // Magic number? input_.gps_course = msg.ground_course; input_.gps_new = true; } From 04c8a46064b0f8a8ae3ee89450136a648b459298 Mon Sep 17 00:00:00 2001 From: BillThePlatypus Date: Mon, 9 Sep 2019 07:41:28 -0600 Subject: [PATCH 2/6] Temporary push to sync w/ other computer --- rosplane/include/estimator_base.h | 8 ++++++-- rosplane/src/estimator_base.cpp | 17 ++++++++++++++--- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/rosplane/include/estimator_base.h b/rosplane/include/estimator_base.h index d2821ae..6e19355 100644 --- a/rosplane/include/estimator_base.h +++ b/rosplane/include/estimator_base.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -91,14 +92,16 @@ class estimator_base ros::NodeHandle nh_; ros::NodeHandle nh_private_; ros::Publisher vehicle_state_pub_; - ros::Subscriber gps_sub_; + ros::Subscriber nav_sat_fix_sub_; + ros::Subscriber twist_stamped_sub_; //used in conjunction with the nav_sat_fix_sub_ ros::Subscriber imu_sub_; ros::Subscriber baro_sub_; ros::Subscriber airspeed_sub_; ros::Subscriber status_sub_; void update(const ros::TimerEvent &); - void gpsCallback(const sensor_msgs::NavSatFix &msg); + void navSatFixCallback(const sensor_msgs::NavSatFix &msg); + void twistStampedCallback(const geometry_msgs::TwistStamped &msg); void imuCallback(const sensor_msgs::Imu &msg); void baroAltCallback(const rosflight_msgs::Barometer &msg); void airspeedCallback(const rosflight_msgs::Airspeed &msg); @@ -107,6 +110,7 @@ class estimator_base double update_rate_; ros::Timer update_timer_; std::string gps_topic_; + std::string vel_topic_; std::string imu_topic_; std::string baro_topic_; std::string airspeed_topic_; diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_base.cpp index e12598a..968f53d 100644 --- a/rosplane/src/estimator_base.cpp +++ b/rosplane/src/estimator_base.cpp @@ -9,7 +9,8 @@ estimator_base::estimator_base(): nh_(ros::NodeHandle()), nh_private_(ros::NodeHandle("~")) { - nh_private_.param("gps_topic", gps_topic_, "navsat_fix"); + nh_private_.param("gps_topic", gps_topic_, "navsat_compat/fix"); + nh_private_.param("vel_topic", vel_topic_, "navsat_compat/vel"); nh_private_.param("imu_topic", imu_topic_, "imu/data"); nh_private_.param("baro_topic", baro_topic_, "baro"); nh_private_.param("airspeed_topic", airspeed_topic_, "airspeed"); @@ -24,7 +25,8 @@ estimator_base::estimator_base(): nh_private_.param("sigma_Vg_gps", params_.sigma_Vg_gps, 0.0500); nh_private_.param("sigma_couse_gps", params_.sigma_course_gps, 0.0045); - gps_sub_ = nh_.subscribe(gps_topic_, 10, &estimator_base::gpsCallback, this); + nav_sat_fix_sub_ = nh_.subscribe(gps_topic_, 10, &estimator_base::navSatFixCallback, this); + twist_stamped_sub_ = nh_.subscribe(gps_topic_, 10, &estimator_base::twistStampedCallback, this); imu_sub_ = nh_.subscribe(imu_topic_, 10, &estimator_base::imuCallback, this); baro_sub_ = nh_.subscribe(baro_topic_, 10, &estimator_base::baroAltCallback, this); airspeed_sub_ = nh_.subscribe(airspeed_topic_, 10, &estimator_base::airspeedCallback, this); @@ -93,7 +95,7 @@ void estimator_base::update(const ros::TimerEvent &) vehicle_state_pub_.publish(msg); } -void estimator_base::gpsCallback(const sensor_msgs::NavSatFix &msg) +void estimator_base::navSatFixCallback(const sensor_msgs::NavSatFix &msg) { bool has_fix = msg.status.status >= sensor_msgs::NavSatStatus::STATUS_FIX; // Higher values refer to augmented fixes if (!has_fix || !std::isfinite(msg.latitude)) @@ -119,6 +121,15 @@ void estimator_base::gpsCallback(const sensor_msgs::NavSatFix &msg) input_.gps_new = true; } } +void estimator_base::twistStampedCallback(const geometry_msgs::TwistStamped &msg) +{ + double v_n = msg.twist.linear.x; + double v_e = msg.twist.linear.y; + double v_d = msg.twist.linear.z; + double ground_speed = sqrt(v_n * v_n + v_e * v_e); + double course = atan2(v_n, -v_e); //TODO check this math. Also, degrees or radians. From North, right? What range? + //TODO finish this function +} void estimator_base::imuCallback(const sensor_msgs::Imu &msg) { From 507dd0bd9269e62984cd68dbd76ee1cef00233bb Mon Sep 17 00:00:00 2001 From: BillThePlatypus Date: Tue, 10 Sep 2019 07:55:49 -0600 Subject: [PATCH 3/6] Progress on updating the gps plugin to work with the new GNSS messages. --- rosplane/src/estimator_base.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_base.cpp index 968f53d..e7e074d 100644 --- a/rosplane/src/estimator_base.cpp +++ b/rosplane/src/estimator_base.cpp @@ -115,9 +115,6 @@ void estimator_base::navSatFixCallback(const sensor_msgs::NavSatFix &msg) input_.gps_n = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0; input_.gps_e = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0; input_.gps_h = msg.altitude - init_alt_; - input_.gps_Vg = msg.speed; - if (msg.speed > 0.3) // Magic number? - input_.gps_course = msg.ground_course; input_.gps_new = true; } } @@ -128,7 +125,9 @@ void estimator_base::twistStampedCallback(const geometry_msgs::TwistStamped &msg double v_d = msg.twist.linear.z; double ground_speed = sqrt(v_n * v_n + v_e * v_e); double course = atan2(v_n, -v_e); //TODO check this math. Also, degrees or radians. From North, right? What range? - //TODO finish this function + input_.gps_Vg = ground_speed; + if(ground_speed > 0.3) //this is a magic number. What is it determined from? + input_.gps_course = course; } void estimator_base::imuCallback(const sensor_msgs::Imu &msg) From fdd5d0222de11844bc4c5250348a5e7993a880dc Mon Sep 17 00:00:00 2001 From: BillThePlatypus Date: Tue, 10 Sep 2019 17:11:57 -0600 Subject: [PATCH 4/6] Fix duplicate subscription causing crash --- rosplane/src/estimator_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_base.cpp index e7e074d..2a4c175 100644 --- a/rosplane/src/estimator_base.cpp +++ b/rosplane/src/estimator_base.cpp @@ -26,7 +26,7 @@ estimator_base::estimator_base(): nh_private_.param("sigma_couse_gps", params_.sigma_course_gps, 0.0045); nav_sat_fix_sub_ = nh_.subscribe(gps_topic_, 10, &estimator_base::navSatFixCallback, this); - twist_stamped_sub_ = nh_.subscribe(gps_topic_, 10, &estimator_base::twistStampedCallback, this); + twist_stamped_sub_ = nh_.subscribe(vel_topic_, 10, &estimator_base::twistStampedCallback, this); imu_sub_ = nh_.subscribe(imu_topic_, 10, &estimator_base::imuCallback, this); baro_sub_ = nh_.subscribe(baro_topic_, 10, &estimator_base::baroAltCallback, this); airspeed_sub_ = nh_.subscribe(airspeed_topic_, 10, &estimator_base::airspeedCallback, this); From 31145dc63dde86270c2670d99f3ec838d1c0cbd8 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 11 Sep 2019 09:05:42 -0600 Subject: [PATCH 5/6] Fix course calculation --- rosplane/src/estimator_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_base.cpp index 2a4c175..075b491 100644 --- a/rosplane/src/estimator_base.cpp +++ b/rosplane/src/estimator_base.cpp @@ -124,7 +124,7 @@ void estimator_base::twistStampedCallback(const geometry_msgs::TwistStamped &msg double v_e = msg.twist.linear.y; double v_d = msg.twist.linear.z; double ground_speed = sqrt(v_n * v_n + v_e * v_e); - double course = atan2(v_n, -v_e); //TODO check this math. Also, degrees or radians. From North, right? What range? + double course = atan2(v_e, v_n); //TODO check this math. Also, degrees or radians. From North, right? What range? input_.gps_Vg = ground_speed; if(ground_speed > 0.3) //this is a magic number. What is it determined from? input_.gps_course = course; From 3401adf49bdabc75c4ace29759e0817bf7108963 Mon Sep 17 00:00:00 2001 From: BillThePlatypus Date: Tue, 17 Sep 2019 11:45:23 -0600 Subject: [PATCH 6/6] Rename variables for clarity --- rosplane/include/estimator_base.h | 12 ++++++------ rosplane/src/estimator_base.cpp | 14 +++++++------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/rosplane/include/estimator_base.h b/rosplane/include/estimator_base.h index 6e19355..07c05c9 100644 --- a/rosplane/include/estimator_base.h +++ b/rosplane/include/estimator_base.h @@ -92,16 +92,16 @@ class estimator_base ros::NodeHandle nh_; ros::NodeHandle nh_private_; ros::Publisher vehicle_state_pub_; - ros::Subscriber nav_sat_fix_sub_; - ros::Subscriber twist_stamped_sub_; //used in conjunction with the nav_sat_fix_sub_ + ros::Subscriber gnss_fix_sub_; + ros::Subscriber gnss_vel_sub_; //used in conjunction with the gnss_fix_sub_ ros::Subscriber imu_sub_; ros::Subscriber baro_sub_; ros::Subscriber airspeed_sub_; ros::Subscriber status_sub_; void update(const ros::TimerEvent &); - void navSatFixCallback(const sensor_msgs::NavSatFix &msg); - void twistStampedCallback(const geometry_msgs::TwistStamped &msg); + void gnssFixCallback(const sensor_msgs::NavSatFix &msg); + void gnssVelCallback(const geometry_msgs::TwistStamped &msg); void imuCallback(const sensor_msgs::Imu &msg); void baroAltCallback(const rosflight_msgs::Barometer &msg); void airspeedCallback(const rosflight_msgs::Airspeed &msg); @@ -109,8 +109,8 @@ class estimator_base double update_rate_; ros::Timer update_timer_; - std::string gps_topic_; - std::string vel_topic_; + std::string gnss_fix_topic_; + std::string gnss_vel_topic_; std::string imu_topic_; std::string baro_topic_; std::string airspeed_topic_; diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_base.cpp index 075b491..2abe806 100644 --- a/rosplane/src/estimator_base.cpp +++ b/rosplane/src/estimator_base.cpp @@ -9,8 +9,8 @@ estimator_base::estimator_base(): nh_(ros::NodeHandle()), nh_private_(ros::NodeHandle("~")) { - nh_private_.param("gps_topic", gps_topic_, "navsat_compat/fix"); - nh_private_.param("vel_topic", vel_topic_, "navsat_compat/vel"); + nh_private_.param("gps_topic", gnss_fix_topic_, "navsat_compat/fix"); + nh_private_.param("vel_topic", gnss_vel_topic_, "navsat_compat/vel"); nh_private_.param("imu_topic", imu_topic_, "imu/data"); nh_private_.param("baro_topic", baro_topic_, "baro"); nh_private_.param("airspeed_topic", airspeed_topic_, "airspeed"); @@ -25,8 +25,8 @@ estimator_base::estimator_base(): nh_private_.param("sigma_Vg_gps", params_.sigma_Vg_gps, 0.0500); nh_private_.param("sigma_couse_gps", params_.sigma_course_gps, 0.0045); - nav_sat_fix_sub_ = nh_.subscribe(gps_topic_, 10, &estimator_base::navSatFixCallback, this); - twist_stamped_sub_ = nh_.subscribe(vel_topic_, 10, &estimator_base::twistStampedCallback, this); + gnss_fix_sub_ = nh_.subscribe(gnss_fix_topic_, 10, &estimator_base::gnssFixCallback, this); + gnss_vel_sub_ = nh_.subscribe(gnss_vel_topic_, 10, &estimator_base::gnssVelCallback, this); imu_sub_ = nh_.subscribe(imu_topic_, 10, &estimator_base::imuCallback, this); baro_sub_ = nh_.subscribe(baro_topic_, 10, &estimator_base::baroAltCallback, this); airspeed_sub_ = nh_.subscribe(airspeed_topic_, 10, &estimator_base::airspeedCallback, this); @@ -95,7 +95,7 @@ void estimator_base::update(const ros::TimerEvent &) vehicle_state_pub_.publish(msg); } -void estimator_base::navSatFixCallback(const sensor_msgs::NavSatFix &msg) +void estimator_base::gnssFixCallback(const sensor_msgs::NavSatFix &msg) { bool has_fix = msg.status.status >= sensor_msgs::NavSatStatus::STATUS_FIX; // Higher values refer to augmented fixes if (!has_fix || !std::isfinite(msg.latitude)) @@ -118,13 +118,13 @@ void estimator_base::navSatFixCallback(const sensor_msgs::NavSatFix &msg) input_.gps_new = true; } } -void estimator_base::twistStampedCallback(const geometry_msgs::TwistStamped &msg) +void estimator_base::gnssVelCallback(const geometry_msgs::TwistStamped &msg) { double v_n = msg.twist.linear.x; double v_e = msg.twist.linear.y; double v_d = msg.twist.linear.z; double ground_speed = sqrt(v_n * v_n + v_e * v_e); - double course = atan2(v_e, v_n); //TODO check this math. Also, degrees or radians. From North, right? What range? + double course = atan2(v_e, v_n); //Does this need to be in a specific range? All uses seem to accept anything. input_.gps_Vg = ground_speed; if(ground_speed > 0.3) //this is a magic number. What is it determined from? input_.gps_course = course;