Skip to content

Commit 8ad3172

Browse files
khallenbeck-dsikhallenbeck-dsi
committed
Add ublox_msg_filters package to time synchronize multiple messages with i_tow to get a single callback
1 parent e540477 commit 8ad3172

File tree

10 files changed

+235
-69
lines changed

10 files changed

+235
-69
lines changed

‎README.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -167,6 +167,7 @@ To publish a given u-blox message to a ROS topic, set the parameter shown below
167167
* `publish/mon/hw`: Topic `~monhw`
168168

169169
### NAV messages
170+
* NAV messages are time stamped with `i_tow`, and multiple messages can be synchronized with [ublox_msg_filters](ublox_msg_filters)
170171
* `publish/nav/all`: This is the default value for the `publish/mon/<message>` parameters below. It defaults to `publish/all`. Individual messages can be enabled or disabled by setting the parameters below.
171172
* `publish/nav/att`: Topic `~navatt`. **ADR/UDR devices only**
172173
* `publish/nav/clock`: Topic `~navclock`

‎ublox/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
<buildtool_depend>ament_cmake</buildtool_depend>
1313
<exec_depend>ublox_serialization</exec_depend>
1414
<exec_depend>ublox_msgs</exec_depend>
15+
<exec_depend>ublox_msg_filters</exec_depend>
1516
<exec_depend>ublox_gps</exec_depend>
1617

1718
<!-- The export tag contains other, unspecified, tags -->

‎ublox_msg_filters/CMakeLists.txt‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(ublox_msg_filters)
3+
4+
# Default to C++14
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 14)
7+
endif()
8+
9+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10+
add_compile_options(-Wall -Wextra -Wpedantic)
11+
endif()
12+
13+
find_package(ament_cmake_ros REQUIRED)
14+
find_package(rclcpp REQUIRED)
15+
find_package(message_filters REQUIRED)
16+
find_package(ublox_msgs REQUIRED)
17+
18+
set(dependencies "rclcpp" "message_filters" "ublox_msgs")
19+
20+
add_executable(talker src/talker.cpp)
21+
ament_target_dependencies(talker ${dependencies})
22+
target_include_directories(talker PUBLIC
23+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
24+
$<INSTALL_INTERFACE:include>
25+
)
26+
27+
add_executable(listener src/listener.cpp)
28+
ament_target_dependencies(listener ${dependencies})
29+
target_include_directories(listener PUBLIC
30+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
31+
$<INSTALL_INTERFACE:include>
32+
)
33+
34+
install(DIRECTORY include/${PROJECT_NAME}/
35+
DESTINATION include/${PROJECT_NAME}
36+
)
37+
install(DIRECTORY launch
38+
DESTINATION share/${PROJECT_NAME}
39+
)
40+
install(TARGETS talker listener
41+
DESTINATION lib/${PROJECT_NAME}
42+
)
43+
44+
if(BUILD_TESTING)
45+
ament_add_gtest(${PROJECT_NAME}-test_exact_time_policy test/test_exact_time_policy.cpp)
46+
ament_target_dependencies(${PROJECT_NAME}-test_exact_time_policy ${dependencies})
47+
target_include_directories(${PROJECT_NAME}-test_exact_time_policy PUBLIC
48+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
49+
$<INSTALL_INTERFACE:include>
50+
)
51+
endif()
52+
53+
ament_export_include_directories(include)
54+
ament_package()

‎ublox_msg_filters/README.md‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
# ublox_msg_filters
2+
3+
Time synchronize multiple uBlox messages to get a single callback
4+
5+
Port of [message_filters::ExactTime](http://wiki.ros.org/message_filters#ExactTime_Policy) message synchronization policy to use integer time of week in milliseconds `i_tow` instead of `ros::Time` in a header.
6+
7+
The [message_filters::ApproximateTime](http://wiki.ros.org/message_filters#ApproximateTime_Policy) message synchronization policy is not relevent because messages generated by a ublox sensor for a single update contain identical `i_tow` time stamps.
8+
9+
See [src/listener.cpp](src/listener.cpp) for example usage.
10+
11+
Launch the example with `ros2 launch ublox_msg_filters example.launch.xml`

‎ublox_msg_filters/include/ublox_msg_filters/exact_time.h‎

Lines changed: 32 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -32,8 +32,8 @@
3232
* POSSIBILITY OF SUCH DAMAGE.
3333
*********************************************************************/
3434

35-
#ifndef MESSAGE_FILTERS__SYNC_EXACT_TIME_H_
36-
#define MESSAGE_FILTERS__SYNC_EXACT_TIME_H_
35+
#ifndef UBLOX_MSG_FILTERS__EXACT_TIME_HPP_
36+
#define UBLOX_MSG_FILTERS__EXACT_TIME_HPP_
3737

3838
#include <deque>
3939
#include <string>
@@ -47,11 +47,33 @@
4747
#include "message_filters/signal9.h"
4848
#include "message_filters/synchronizer.h"
4949

50-
namespace message_filters
50+
namespace ublox_msg_filters
5151
{
5252
namespace sync_policies
5353
{
5454

55+
using NullType = message_filters::NullType;
56+
using Connection = message_filters::Connection;
57+
58+
template<typename M0, typename M1, typename M2, typename M3, typename M4,
59+
typename M5, typename M6, typename M7, typename M8>
60+
using PolicyBase = message_filters::PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>;
61+
62+
template<class Policy>
63+
using Synchronizer = message_filters::Synchronizer<Policy>;
64+
65+
template<typename M>
66+
struct iTOW
67+
{
68+
static u_int32_t value(const M& m) { return m.i_tow; }
69+
};
70+
71+
template<>
72+
struct iTOW<NullType>
73+
{
74+
static u_int32_t value(const NullType&) { return 0; }
75+
};
76+
5577
template<typename M0, typename M1, typename M2 = NullType, typename M3 = NullType, typename M4 = NullType,
5678
typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, typename M8 = NullType>
5779
struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
@@ -76,6 +98,7 @@ struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
7698
ExactTime(uint32_t queue_size)
7799
: parent_(0)
78100
, queue_size_(queue_size)
101+
, last_signal_time_(0)
79102
{
80103
}
81104

@@ -108,7 +131,7 @@ struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
108131

109132
std::lock_guard<std::mutex> lock(mutex_);
110133

111-
Tuple& t = tuples_[mt::TimeStamp<typename std::tuple_element<i, Messages>::type>::value(*evt.getMessage())];
134+
Tuple& t = tuples_[iTOW<typename std::tuple_element<i, Messages>::type>::value(*evt.getMessage())];
112135
std::get<i>(t) = evt;
113136

114137
checkTuple(t);
@@ -162,7 +185,7 @@ struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
162185
std::get<3>(t), std::get<4>(t), std::get<5>(t),
163186
std::get<6>(t), std::get<7>(t), std::get<8>(t));
164187

165-
last_signal_time_ = mt::TimeStamp<M0>::value(*std::get<0>(t).getMessage());
188+
last_signal_time_ = iTOW<M0>::value(*std::get<0>(t).getMessage());
166189

167190
tuples_.erase(last_signal_time_);
168191

@@ -212,17 +235,17 @@ struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
212235
Sync* parent_;
213236

214237
uint32_t queue_size_;
215-
typedef std::map<rclcpp::Time, Tuple> M_TimeToTuple;
238+
typedef std::map<uint32_t, Tuple> M_TimeToTuple;
216239
M_TimeToTuple tuples_;
217-
rclcpp::Time last_signal_time_;
240+
uint32_t last_signal_time_;
218241

219242
Signal drop_signal_;
220243

221244
std::mutex mutex_;
222245
};
223246

224247
} // namespace sync_policies
225-
} // namespace message_filters
248+
} // namespace ublox_msg_filters
226249

227-
#endif // MESSAGE_FILTERS__SYNC_EXACT_TIME_H_
250+
#endif // UBLOX_MSG_FILTERS__EXACT_TIME_HPP_
228251

‎ublox_msg_filters/launch/example.launch.xml‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<?xml version="1.0"?>
2+
<launch>
3+
<!-- Generate and transmit messages -->
4+
<node pkg="ublox_msg_filters" exec="talker" name="talker" output="screen" />
5+
6+
<!-- Receive and sync messages -->
7+
<node pkg="ublox_msg_filters" exec="listener" name="listener" output="screen" />
8+
9+
</launch>

‎ublox_msg_filters/package.xml‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>ublox_msg_filters</name>
5+
<version>2.3.0</version>
6+
<description>Time synchronize multiple uBlox messages to get a single callback</description>
7+
<author>Kevin Hallenbeck</author>
8+
<maintainer email="[email protected]">Kevin Hallenbeck</maintainer>
9+
<license>BSD</license>
10+
<url>http://ros.org/wiki/ublox</url>
11+
12+
<buildtool_depend>ament_cmake_ros</buildtool_depend>
13+
14+
<depend>rclcpp</depend>
15+
<depend>message_filters</depend>
16+
<depend>ublox_msgs</depend>
17+
18+
<export>
19+
<build_type>ament_cmake</build_type>
20+
</export>
21+
</package>

‎ublox_msg_filters/src/listener.cpp‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
#include <rclcpp/rclcpp.hpp>
2+
#include <ublox_msgs/msg/nav_posllh.hpp>
3+
#include <ublox_msgs/msg/nav_relposned9.hpp>
4+
#include <ublox_msgs/msg/nav_velned.hpp>
5+
#include <ublox_msg_filters/exact_time.h>
6+
#include <message_filters/subscriber.h>
7+
8+
class Listener : public rclcpp::Node {
9+
public:
10+
Listener() : Node("listener"),
11+
sub1_(this, "msg1"),
12+
sub2_(this, "msg2"),
13+
sub3_(this, "msg3"),
14+
sync_(MySyncPolicy(10), sub1_, sub2_, sub3_)
15+
{
16+
using std::placeholders::_1;
17+
using std::placeholders::_2;
18+
using std::placeholders::_3;
19+
sync_.registerCallback(std::bind(&Listener::callback, this, _1, _2, _3));
20+
RCLCPP_INFO(this->get_logger(), "Ready to receive");
21+
}
22+
23+
private:
24+
void callback(const ublox_msgs::msg::NavPOSLLH::ConstSharedPtr msg1,
25+
const ublox_msgs::msg::NavRELPOSNED9::ConstSharedPtr msg2,
26+
const ublox_msgs::msg::NavVELNED::ConstSharedPtr msg3) {
27+
RCLCPP_INFO(this->get_logger(), "RX %u %u %u", msg1->i_tow, msg2->i_tow, msg3->i_tow);
28+
}
29+
30+
private:
31+
message_filters::Subscriber<ublox_msgs::msg::NavPOSLLH> sub1_;
32+
message_filters::Subscriber<ublox_msgs::msg::NavRELPOSNED9> sub2_;
33+
message_filters::Subscriber<ublox_msgs::msg::NavVELNED> sub3_;
34+
typedef ublox_msg_filters::sync_policies::ExactTime<ublox_msgs::msg::NavPOSLLH,
35+
ublox_msgs::msg::NavRELPOSNED9,
36+
ublox_msgs::msg::NavVELNED> MySyncPolicy;
37+
message_filters::Synchronizer<MySyncPolicy> sync_;
38+
};
39+
40+
int main(int argc, char * argv[])
41+
{
42+
rclcpp::init(argc,argv);
43+
rclcpp::spin(std::make_shared<Listener>());
44+
rclcpp::shutdown();
45+
return 0;
46+
}

‎ublox_msg_filters/src/talker.cpp‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
#include <rclcpp/rclcpp.hpp>
2+
#include <ublox_msgs/msg/nav_posllh.hpp>
3+
#include <ublox_msgs/msg/nav_relposned9.hpp>
4+
#include <ublox_msgs/msg/nav_velned.hpp>
5+
6+
class Talker : public rclcpp::Node {
7+
public:
8+
Talker() : Node("talker") {
9+
using namespace std::chrono_literals;
10+
pub1_ = this->create_publisher<ublox_msgs::msg::NavPOSLLH>("msg1", 2);
11+
pub2_ = this->create_publisher<ublox_msgs::msg::NavRELPOSNED9>("msg2", 2);
12+
pub3_ = this->create_publisher<ublox_msgs::msg::NavVELNED>("msg3", 2);
13+
timer_ = this->create_wall_timer(0.2s, std::bind(&Talker::publish, this));
14+
RCLCPP_INFO(this->get_logger(), "Ready to transmit");
15+
}
16+
17+
private:
18+
void publish() {
19+
RCLCPP_INFO(this->get_logger(), "TX %u %u %u", i_tow_, i_tow_ + 1, i_tow_ + 3);
20+
21+
auto msg1 = ublox_msgs::msg::NavPOSLLH();
22+
msg1.i_tow = i_tow_;
23+
pub1_->publish(msg1);
24+
25+
auto msg2 = ublox_msgs::msg::NavRELPOSNED9();
26+
msg2.i_tow = i_tow_ + 1;
27+
pub2_->publish(msg2);
28+
29+
auto msg3 = ublox_msgs::msg::NavVELNED();
30+
msg3.i_tow = i_tow_ + 3;
31+
pub3_->publish(msg3);
32+
33+
i_tow_++;
34+
}
35+
36+
private:
37+
rclcpp::TimerBase::SharedPtr timer_;
38+
rclcpp::Publisher<ublox_msgs::msg::NavPOSLLH>::SharedPtr pub1_;
39+
rclcpp::Publisher<ublox_msgs::msg::NavRELPOSNED9>::SharedPtr pub2_;
40+
rclcpp::Publisher<ublox_msgs::msg::NavVELNED>::SharedPtr pub3_;
41+
uint32_t i_tow_ = 0;
42+
};
43+
44+
int main(int argc, char * argv[])
45+
{
46+
rclcpp::init(argc, argv);
47+
rclcpp::spin(std::make_shared<Talker>());
48+
rclcpp::shutdown();
49+
return 0;
50+
}

0 commit comments

Comments
 (0)