niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Use rosgraph_msgs/Clock for /clock topic. (#474)

Browse source 
* Use rosgraph_msgs/Clock for /clock topic.

* Update the test cases.
This commit is contained in:
Ernesto Corbellini 2018-05-30 20:32:08 -03:00 • committed by Mikael Arguedas
parent 4efcd330fe
commit d6057270f2
5 changed files with 29 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

3
rclcpp/CMakeLists.txt
View file

@ -8,6 +8,7 @@ find_package(rcl REQUIRED)
find_package(rcl_interfaces REQUIRED)
find_package(rmw REQUIRED)
find_package(rmw_implementation REQUIRED)
find_package(rosgraph_msgs REQUIRED)
find_package(rosidl_generator_cpp REQUIRED)
find_package(rosidl_typesupport_c REQUIRED)
find_package(rosidl_typesupport_cpp REQUIRED)
@ -99,6 +100,7 @@ add_library(${PROJECT_NAME}
ament_target_dependencies(${PROJECT_NAME}
"rcl"
"builtin_interfaces"
"rosgraph_msgs"
"rosidl_typesupport_cpp"
"rosidl_generator_cpp")
@ -121,6 +123,7 @@ ament_export_libraries(${PROJECT_NAME})
ament_export_dependencies(ament_cmake)
ament_export_dependencies(rcl)
ament_export_dependencies(builtin_interfaces)
ament_export_dependencies(rosgraph_msgs)
ament_export_dependencies(rosidl_typesupport_cpp)
ament_export_dependencies(rosidl_typesupport_c)
ament_export_dependencies(rosidl_generator_cpp)

7
rclcpp/include/rclcpp/time_source.hpp
View file

@ -21,6 +21,7 @@
#include "rcl/time.h"
#include "builtin_interfaces/msg/time.hpp"
#include "rosgraph_msgs/msg/clock.hpp"
#include "rcl_interfaces/msg/parameter_event.hpp"
#include "rclcpp/node.hpp"
@ -75,13 +76,13 @@ private:
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
// The subscription for the clock callback
using MessageT = builtin_interfaces::msg::Time;
using MessageT = rosgraph_msgs::msg::Clock;
using Alloc = std::allocator<void>;
using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
std::shared_ptr<SubscriptionT> clock_subscription_;
// The clock callback itself
void clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg);
void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg);
// Parameter Client pointer
std::shared_ptr<rclcpp::AsyncParametersClient> parameter_client_;
@ -115,7 +116,7 @@ private:
// This is needed when new clocks are added.
bool ros_time_active_;
// Last set message to be passed to newly registered clocks
builtin_interfaces::msg::Time::SharedPtr last_msg_set_;
rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_;
// A lock to protect iterating the associated_clocks_ field.
std::mutex clock_list_lock_;

2
rclcpp/package.xml
View file

@ -13,11 +13,13 @@
<build_depend>builtin_interfaces</build_depend>
<build_depend>rcl_interfaces</build_depend>
<build_depend>rosgraph_msgs</build_depend>
<build_depend>rosidl_generator_cpp</build_depend>
<build_depend>rosidl_typesupport_c</build_depend>
<build_depend>rosidl_typesupport_cpp</build_depend>
<build_export_depend>builtin_interfaces</build_export_depend>
<build_export_depend>rcl_interfaces</build_export_depend>
<build_export_depend>rosgraph_msgs</build_export_depend>
<build_export_depend>rosidl_generator_cpp</build_export_depend>
<build_export_depend>rosidl_typesupport_c</build_export_depend>
<build_export_depend>rosidl_typesupport_cpp</build_export_depend>

8
rclcpp/src/rclcpp/time_source.cpp
View file

@ -118,7 +118,8 @@ void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
associated_clocks_.push_back(clock);
// Set the clock if there's already data for it
if (last_msg_set_) {
set_clock(last_msg_set_, ros_time_active_, clock);
auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
set_clock(time_msg, ros_time_active_, clock);
}
}
@ -194,17 +195,18 @@ void TimeSource::set_clock(
clock->invoke_postjump_callbacks(active_callbacks, jump);
}
void TimeSource::clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg)
void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg)
{
if (!this->ros_time_active_) {
enable_ros_time();
}
// Cache the last message in case a new clock is attached.
last_msg_set_ = msg;
auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);
std::lock_guard<std::mutex> guard(clock_list_lock_);
for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
set_clock(msg, true, *it);
set_clock(time_msg, true, *it);
}
}

34
rclcpp/test/test_time_source.cpp
View file

@ -92,18 +92,17 @@ TEST_F(TestTimeSource, clock) {
ts.attachClock(ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());
auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
rmw_qos_profile_default);
rclcpp::WallRate loop_rate(50);
for (int i = 0; i < 5; ++i) {
if (!rclcpp::ok()) {
break; // Break for ctrl-c
}
auto msg = std::make_shared<builtin_interfaces::msg::Time>();
msg->sec = i;
msg->nanosec = 1000;
auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
msg->clock.sec = i;
msg->clock.nanosec = 1000;
clock_pub->publish(msg);
// std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
rclcpp::spin_some(node);
loop_rate.sleep();
}
@ -162,7 +161,7 @@ TEST_F(TestTimeSource, callbacks) {
ts.attachClock(ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());
auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
rmw_qos_profile_default);
rclcpp::WallRate loop_rate(50);
@ -170,11 +169,10 @@ TEST_F(TestTimeSource, callbacks) {
if (!rclcpp::ok()) {
break; // Break for ctrl-c
}
auto msg = std::make_shared<builtin_interfaces::msg::Time>();
msg->sec = i;
msg->nanosec = 1000;
auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
msg->clock.sec = i;
msg->clock.nanosec = 1000;
clock_pub->publish(msg);
// std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
rclcpp::spin_some(node);
loop_rate.sleep();
}
@ -204,11 +202,10 @@ TEST_F(TestTimeSource, callbacks) {
if (!rclcpp::ok()) {
break; // Break for ctrl-c
}
auto msg = std::make_shared<builtin_interfaces::msg::Time>();
msg->sec = i;
msg->nanosec = 2000;
auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
msg->clock.sec = i;
msg->clock.nanosec = 2000;
clock_pub->publish(msg);
// std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
rclcpp::spin_some(node);
loop_rate.sleep();
}
@ -229,7 +226,7 @@ TEST_F(TestTimeSource, callbacks) {
void trigger_clock_changes(
rclcpp::Node::SharedPtr node)
{
auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
rmw_qos_profile_default);
rclcpp::executors::SingleThreadedExecutor executor;
@ -240,11 +237,10 @@ void trigger_clock_changes(
if (!rclcpp::ok()) {
break; // Break for ctrl-c
}
auto msg = std::make_shared<builtin_interfaces::msg::Time>();
msg->sec = i;
msg->nanosec = 1000;
auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
msg->clock.sec = i;
msg->clock.nanosec = 1000;
clock_pub->publish(msg);
// std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
executor.spin_once(1000000ns);
loop_rate.sleep();
}