Skip to content

Commit 88673b7

Browse files
kagibsonkagibson
authored
Add position and velocity covariance message (UBX_NAV_COV) (#196)
* Add ubx-nav-cov message type * Add navcov message * Subscribe to message * Add serializer
1 parent 71e2f0c commit 88673b7

File tree

7 files changed

+93
-2
lines changed

7 files changed

+93
-2
lines changed

‎ublox_gps/include/ublox_gps/node.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -265,6 +265,7 @@ class UbloxNode final : public rclcpp::Node {
265265
rclcpp::Publisher<ublox_msgs::msg::NavSTATUS>::SharedPtr nav_status_pub_;
266266
rclcpp::Publisher<ublox_msgs::msg::NavPOSECEF>::SharedPtr nav_posecef_pub_;
267267
rclcpp::Publisher<ublox_msgs::msg::NavCLOCK>::SharedPtr nav_clock_pub_;
268+
rclcpp::Publisher<ublox_msgs::msg::NavCOV>::SharedPtr nav_cov_pub_;
268269
rclcpp::Publisher<ublox_msgs::msg::AidALM>::SharedPtr aid_alm_pub_;
269270
rclcpp::Publisher<ublox_msgs::msg::AidEPH>::SharedPtr aid_eph_pub_;
270271
rclcpp::Publisher<ublox_msgs::msg::AidHUI>::SharedPtr aid_hui_pub_;

‎ublox_gps/src/node.cpp‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@
5252
#include <ublox_msgs/msg/inf.hpp>
5353
#include <ublox_msgs/msg/mon_ver.hpp>
5454
#include <ublox_msgs/msg/nav_clock.hpp>
55+
#include <ublox_msgs/msg/nav_cov.hpp>
5556
#include <ublox_msgs/msg/nav_posecef.hpp>
5657
#include <ublox_msgs/msg/nav_status.hpp>
5758

@@ -410,6 +411,7 @@ void UbloxNode::getRosParams() {
410411
this->declare_parameter("publish.nav.all", getRosBoolean(this, "publish.all"));
411412
this->declare_parameter("publish.nav.att", getRosBoolean(this, "publish.nav.all"));
412413
this->declare_parameter("publish.nav.clock", getRosBoolean(this, "publish.nav.all"));
414+
this->declare_parameter("publish.nav.cov", getRosBoolean(this, "publish.nav.all"));
413415
this->declare_parameter("publish.nav.heading", getRosBoolean(this, "publish.nav.all"));
414416
this->declare_parameter("publish.nav.posecef", getRosBoolean(this, "publish.nav.all"));
415417
this->declare_parameter("publish.nav.posllh", getRosBoolean(this, "publish.nav.all"));
@@ -472,8 +474,8 @@ void UbloxNode::getRosParams() {
472474
if (getRosBoolean(this, "publish.nav.posecef")) {
473475
nav_posecef_pub_ = this->create_publisher<ublox_msgs::msg::NavPOSECEF>("navposecef", 1);
474476
}
475-
if (getRosBoolean(this, "publish.nav.clock")) {
476-
nav_clock_pub_ = this->create_publisher<ublox_msgs::msg::NavCLOCK>("navclock", 1);
477+
if (getRosBoolean(this, "publish.nav.cov")) {
478+
nav_cov_pub_ = this->create_publisher<ublox_msgs::msg::NavCOV>("navcov", 1);
477479
}
478480
if (getRosBoolean(this, "publish.nav.clock")) {
479481
nav_clock_pub_ = this->create_publisher<ublox_msgs::msg::NavCLOCK>("navclock", 1);
@@ -547,6 +549,11 @@ void UbloxNode::subscribe() {
547549
1);
548550
}
549551

552+
if (getRosBoolean(this, "publish.nav.cov")) {
553+
gps_->subscribe<ublox_msgs::msg::NavCOV>([this](const ublox_msgs::msg::NavCOV &m) { nav_cov_pub_->publish(m); },
554+
1);
555+
}
556+
550557
// INF messages
551558
if (getRosBoolean(this, "inf.debug")) {
552559
gps_->subscribeId<ublox_msgs::msg::Inf>(

‎ublox_msgs/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,7 @@ set(msg_files
5858
"msg/MonVER.msg"
5959
"msg/NavATT.msg"
6060
"msg/NavCLOCK.msg"
61+
"msg/NavCOV.msg"
6162
"msg/NavDGPS.msg"
6263
"msg/NavDGPSSV.msg"
6364
"msg/NavDOP.msg"

‎ublox_msgs/include/ublox_msgs/serialization.hpp‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1504,6 +1504,56 @@ struct UbloxSerializer<ublox_msgs::msg::NavCLOCK_<ContainerAllocator> > {
15041504
}
15051505
};
15061506

1507+
template <typename ContainerAllocator>
1508+
struct UbloxSerializer<ublox_msgs::msg::NavCOV_<ContainerAllocator> > {
1509+
inline static void read(const uint8_t *data, uint32_t count,
1510+
ublox_msgs::msg::NavCOV_<ContainerAllocator> & m) {
1511+
UbloxIStream stream(const_cast<uint8_t *>(data), count);
1512+
stream.next(m.i_tow);
1513+
stream.next(m.pos_cov_valid);
1514+
stream.next(m.vel_cov_valid);
1515+
stream.next(m.reserved_0);
1516+
stream.next(m.pos_cov_nn);
1517+
stream.next(m.pos_cov_ne);
1518+
stream.next(m.pos_cov_nd);
1519+
stream.next(m.pos_cov_ee);
1520+
stream.next(m.pos_cov_ed);
1521+
stream.next(m.pos_cov_dd);
1522+
stream.next(m.vel_cov_nn);
1523+
stream.next(m.vel_cov_ne);
1524+
stream.next(m.vel_cov_nd);
1525+
stream.next(m.vel_cov_ee);
1526+
stream.next(m.vel_cov_ed);
1527+
stream.next(m.vel_cov_dd);
1528+
}
1529+
1530+
inline static uint32_t serializedLength(const ublox_msgs::msg::NavCOV_<ContainerAllocator> & m) {
1531+
(void)m;
1532+
return 64;
1533+
}
1534+
1535+
inline static void write(uint8_t *data, uint32_t size,
1536+
const ublox_msgs::msg::NavCOV_<ContainerAllocator> & m) {
1537+
UbloxOStream stream(data, size);
1538+
stream.next(m.i_tow);
1539+
stream.next(m.pos_cov_valid);
1540+
stream.next(m.vel_cov_valid);
1541+
stream.next(m.reserved_0);
1542+
stream.next(m.pos_cov_nn);
1543+
stream.next(m.pos_cov_ne);
1544+
stream.next(m.pos_cov_nd);
1545+
stream.next(m.pos_cov_ee);
1546+
stream.next(m.pos_cov_ed);
1547+
stream.next(m.pos_cov_dd);
1548+
stream.next(m.vel_cov_nn);
1549+
stream.next(m.vel_cov_ne);
1550+
stream.next(m.vel_cov_nd);
1551+
stream.next(m.vel_cov_ee);
1552+
stream.next(m.vel_cov_ed);
1553+
stream.next(m.vel_cov_dd);
1554+
}
1555+
};
1556+
15071557
template <typename ContainerAllocator>
15081558
struct UbloxSerializer<ublox_msgs::msg::NavDGPSSV_<ContainerAllocator> > {
15091559
inline static void read(UbloxIStream& stream, ublox_msgs::msg::NavDGPSSV_<ContainerAllocator> & m) {

‎ublox_msgs/include/ublox_msgs/ublox_msgs.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@
3131

3232
#include <ublox_msgs/msg/nav_att.hpp>
3333
#include <ublox_msgs/msg/nav_clock.hpp>
34+
#include <ublox_msgs/msg/nav_cov.hpp>
3435
#include <ublox_msgs/msg/nav_dgps.hpp>
3536
#include <ublox_msgs/msg/nav_dop.hpp>
3637
#include <ublox_msgs/msg/nav_posecef.hpp>
@@ -154,6 +155,7 @@ namespace Message {
154155
namespace NAV {
155156
static const uint8_t ATT = ublox_msgs::msg::NavATT::MESSAGE_ID;
156157
static const uint8_t CLOCK = ublox_msgs::msg::NavCLOCK::MESSAGE_ID;
158+
static const uint8_t COV = ublox_msgs::msg::NavCOV::MESSAGE_ID;
157159
static const uint8_t DGPS = ublox_msgs::msg::NavDGPS::MESSAGE_ID;
158160
static const uint8_t DOP = ublox_msgs::msg::NavDOP::MESSAGE_ID;
159161
static const uint8_t POSECEF = ublox_msgs::msg::NavPOSECEF::MESSAGE_ID;

‎ublox_msgs/msg/NavCOV.msg‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
# NAV-COV (0x01 0x36)
2+
# Covariance matrices for position and velocity solutions
3+
#
4+
5+
uint8 CLASS_ID = 1
6+
uint8 MESSAGE_ID = 54
7+
8+
uint32 i_tow # GPS Millisecond time of week [ms]
9+
10+
uint8 version # Message version (0x00 for this version)
11+
uint8 pos_cov_valid # Position covariance matrix validity flag
12+
uint8 vel_cov_valid # Velocity covariance matrix validity flag
13+
uint8[9] reserved_0 # Reserved
14+
15+
float32 pos_cov_nn # Position covariance matrix value p_NN [m^2]
16+
float32 pos_cov_ne # Position covariance matrix value p_NE [m^2]
17+
float32 pos_cov_nd # Position covariance matrix value p_ND [m^2]
18+
float32 pos_cov_ee # Position covariance matrix value p_EE [m^2]
19+
float32 pos_cov_ed # Position covariance matrix value p_ED [m^2]
20+
float32 pos_cov_dd # Position covariance matrix value p_DD [m^2]
21+
22+
23+
float32 vel_cov_nn # Velocity covariance matrix value v_NN [m^2/s^2]
24+
float32 vel_cov_ne # Velocity covariance matrix value v_NE [m^2/s^2]
25+
float32 vel_cov_nd # Velocity covariance matrix value v_ND [m^2/s^2]
26+
float32 vel_cov_ee # Velocity covariance matrix value v_EE [m^2/s^2]
27+
float32 vel_cov_ed # Velocity covariance matrix value v_ED [m^2/s^2]
28+
float32 vel_cov_dd # Velocity covariance matrix value v_DD [m^2/s^2]

‎ublox_msgs/src/ublox_msgs.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,8 @@ DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT,
3838
ublox_msgs, NavATT)
3939
DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK,
4040
ublox_msgs, NavCLOCK)
41+
DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::COV,
42+
ublox_msgs, NavCOV)
4143
DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS,
4244
ublox_msgs, NavDGPS)
4345
DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP,

0 commit comments

Comments
 (0)