Skip to content

Commit 190836c

Browse files
mdurrani808mdurrani808
authored
Fix GPS heading publishing. (#86)
* fix heading publishing' * misread the docs on float64 * adhere to ros gps convention * add raw compass bearing * add to package.xml
1 parent 18184dd commit 190836c

5 files changed

Lines changed: 44 additions & 37 deletions

File tree

‎src/msgs/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
1919
"msg/Currentdraw.msg"
2020
"msg/JointReceiver.msg"
2121
"msg/SystemInfo.msg"
22+
"msg/Heading.msg"
2223
"srv/SetController.srv"
2324
"action/NavigateToGPS.action"
2425
DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces action_msgs

‎src/msgs/msg/Heading.msg‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
# heading: ROS convention (0=East, CCW, radians). Converted from compass bearing,
2+
# but does not account for magnetic declination.
3+
# compass_bearing: raw magnetometer reading (0=magnetic North, CW, degrees).
4+
5+
std_msgs/Header header
6+
float64 heading
7+
float64 heading_acc
8+
float64 compass_bearing

‎src/subsystems/navigation/athena_gps/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED)
1414
find_package(rclcpp REQUIRED)
1515
find_package(std_msgs REQUIRED)
1616
find_package(sensor_msgs REQUIRED)
17+
find_package(msgs REQUIRED)
1718
find_package(MAVSDK REQUIRED)
1819
add_executable(athena_gps src/PixhawkNode.cpp)
1920

@@ -26,6 +27,7 @@ ament_target_dependencies(athena_gps
2627
rclcpp
2728
std_msgs
2829
sensor_msgs
30+
msgs
2931
)
3032
target_link_libraries(athena_gps
3133
MAVSDK::mavsdk

‎src/subsystems/navigation/athena_gps/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
<depend>rclcpp</depend>
1313
<depend>std_msgs</depend>
1414
<depend>sensor_msgs</depend>
15+
<depend>msgs</depend>
1516

1617
<test_depend>ament_lint_auto</test_depend>
1718
<test_depend>ament_lint_common</test_depend>

‎src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp‎

Lines changed: 32 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77
#include "rclcpp/rclcpp.hpp"
88
#include "sensor_msgs/msg/imu.hpp"
99
#include "sensor_msgs/msg/nav_sat_fix.hpp"
10-
#include "std_msgs/msg/float64.hpp"
10+
#include "msgs/msg/heading.hpp"
1111
#include <mavsdk/mavsdk.h>
1212
#include <mavsdk/plugins/telemetry/telemetry.h>
1313

@@ -54,7 +54,7 @@ class PixhawkNode : public rclcpp::Node
5454
// Create publishers unconditionally so they're always valid
5555
imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_topic, 10);
5656
gps_publisher_ = this->create_publisher<sensor_msgs::msg::NavSatFix>(gps_topic, 10);
57-
heading_publisher_ = this->create_publisher<std_msgs::msg::Float64>(heading_topic, 10);
57+
heading_publisher_ = this->create_publisher<msgs::msg::Heading>(heading_topic, 10);
5858

5959
// Create MAVSDK instance
6060
mavsdk_ = std::make_unique<Mavsdk>(Mavsdk::Configuration{
@@ -78,7 +78,7 @@ class PixhawkNode : public rclcpp::Node
7878

7979
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
8080
rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_publisher_;
81-
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr heading_publisher_;
81+
rclcpp::Publisher<msgs::msg::Heading>::SharedPtr heading_publisher_;
8282
rclcpp::TimerBase::SharedPtr init_timer_;
8383

8484
Telemetry::GpsInfo last_gps_info_;
@@ -94,12 +94,6 @@ class PixhawkNode : public rclcpp::Node
9494
bool is_initialized_;
9595
bool connection_added_;
9696

97-
void publish_heading() {
98-
auto msg = std_msgs::msg::Float64();
99-
msg.data = last_heading_deg_;
100-
heading_publisher_->publish(msg);
101-
}
102-
10397
void try_initialize() {
10498
if (is_initialized_) {
10599
return;
@@ -151,7 +145,6 @@ class PixhawkNode : public rclcpp::Node
151145

152146
RCLCPP_INFO(this->get_logger(), "Telemetry rates configured successfully");
153147

154-
// Subscribe to telemetry streams
155148
telemetry_->subscribe_imu([this](const Telemetry::Imu &imu) {
156149
imu_callback(imu);
157150
});
@@ -160,11 +153,6 @@ class PixhawkNode : public rclcpp::Node
160153
last_attitude_quaternion_ = quaternion;
161154
});
162155

163-
telemetry_->subscribe_heading([this](Telemetry::Heading heading) {
164-
last_heading_deg_ = heading.heading_deg;
165-
publish_heading();
166-
});
167-
168156
telemetry_->subscribe_raw_gps([this](const Telemetry::RawGps &raw_gps) {
169157
raw_gps_callback(raw_gps);
170158
});
@@ -228,39 +216,39 @@ class PixhawkNode : public rclcpp::Node
228216
}
229217

230218
void raw_gps_callback(const Telemetry::RawGps raw_gps) {
231-
auto msg = sensor_msgs::msg::NavSatFix();
219+
auto gps_msg = sensor_msgs::msg::NavSatFix();
232220

233-
msg.header.stamp = this->now();
234-
msg.header.frame_id = gps_frame_id_;
221+
gps_msg.header.stamp = this->now();
222+
gps_msg.header.frame_id = gps_frame_id_;
235223

236-
msg.latitude = raw_gps.latitude_deg;
237-
msg.longitude = raw_gps.longitude_deg;
238-
msg.altitude = raw_gps.altitude_ellipsoid_m;
224+
gps_msg.latitude = raw_gps.latitude_deg;
225+
gps_msg.longitude = raw_gps.longitude_deg;
226+
gps_msg.altitude = raw_gps.altitude_ellipsoid_m;
239227

240228
// Map GPS fix to ROS fix
241229
if (last_gps_info_.num_satellites > 0) {
242230
switch (last_gps_info_.fix_type) {
243231
case Telemetry::FixType::NoGps:
244232
case Telemetry::FixType::NoFix:
245-
msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
233+
gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
246234
break;
247235
case Telemetry::FixType::Fix2D:
248-
msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
236+
gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
249237
break;
250238
case Telemetry::FixType::Fix3D:
251239
case Telemetry::FixType::FixDgps:
252240
case Telemetry::FixType::RtkFloat:
253241
case Telemetry::FixType::RtkFixed:
254-
msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX;
242+
gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX;
255243
break;
256244
default:
257-
msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
245+
gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
258246
}
259247
} else {
260-
msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
248+
gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
261249
}
262250

263-
msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS;
251+
gps_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS;
264252

265253
// calcilate variances
266254
if (!std::isnan(raw_gps.horizontal_uncertainty_m) &&
@@ -269,23 +257,30 @@ class PixhawkNode : public rclcpp::Node
269257
double h_variance = raw_gps.horizontal_uncertainty_m * raw_gps.horizontal_uncertainty_m;
270258
double v_variance = raw_gps.vertical_uncertainty_m * raw_gps.vertical_uncertainty_m;
271259

272-
msg.position_covariance[0] = h_variance;
273-
msg.position_covariance[4] = h_variance;
274-
msg.position_covariance[8] = v_variance;
260+
gps_msg.position_covariance[0] = h_variance;
261+
gps_msg.position_covariance[4] = h_variance;
262+
gps_msg.position_covariance[8] = v_variance;
275263

276264
// refine estimate using doppler
277265
if (!std::isnan(raw_gps.hdop) && !std::isnan(raw_gps.vdop)) {
278-
msg.position_covariance[0] = h_variance * (raw_gps.hdop / 2.0);
279-
msg.position_covariance[4] = h_variance * (raw_gps.hdop / 2.0);
280-
msg.position_covariance[8] = v_variance * raw_gps.vdop;
266+
gps_msg.position_covariance[0] = h_variance * (raw_gps.hdop / 2.0);
267+
gps_msg.position_covariance[4] = h_variance * (raw_gps.hdop / 2.0);
268+
gps_msg.position_covariance[8] = v_variance * raw_gps.vdop;
281269
}
282270

283-
msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
271+
gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
284272
} else {
285-
msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
273+
gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
286274
}
287-
288-
gps_publisher_->publish(msg);
275+
auto heading_msg = msgs::msg::Heading();
276+
heading_msg.header.stamp = this->now();
277+
heading_msg.compass_bearing = raw_gps.yaw_deg;
278+
// Convert from compass bearing (0=magnetic North, CW, degrees) to ROS convention (0=East, CCW, radians)
279+
heading_msg.heading = (90.0 - raw_gps.yaw_deg) * M_PI / 180.0;
280+
heading_msg.heading_acc = raw_gps.heading_uncertainty_deg * M_PI / 180.0;
281+
282+
heading_publisher_->publish(heading_msg);
283+
gps_publisher_->publish(gps_msg);
289284
}
290285
};
291286

0 commit comments

Comments
 (0)