6 #include <gtest/gtest.h>
7 #include <std_msgs/msg/string.hpp>
8 #include <std_srvs/srv/set_bool.hpp>
9 #include <websocketpp/config/asio_client.hpp>
15 constexpr
char URI[] =
"ws://localhost:8765";
18 constexpr uint8_t
HELLO_WORLD_CDR[] = {0, 1, 0, 0, 12, 0, 0, 0, 104, 101,
19 108, 108, 111, 32, 119, 111, 114, 108, 100, 0};
39 rclcpp::executors::SingleThreadedExecutor
executor;
49 inline static const std::string
PARAM_1_NAME =
"string_param";
55 inline static const std::string
PARAM_2_NAME =
"int_array_param";
63 inline static const std::string
PARAM_4_NAME =
"float_array_param";
68 auto nodeOptions = rclcpp::NodeOptions();
69 nodeOptions.allow_undeclared_parameters(
true);
71 auto p1Param = rcl_interfaces::msg::ParameterDescriptor{};
73 p1Param.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
74 p1Param.read_only =
false;
79 auto p2Param = rcl_interfaces::msg::ParameterDescriptor{};
81 p2Param.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY;
82 p2Param.read_only =
false;
90 _wsClient = std::make_shared<foxglove::Client<websocketpp::config::asio_client>>();
101 std::shared_ptr<foxglove::Client<websocketpp::config::asio_client>>
_wsClient;
106 inline static const std::string
SERVICE_NAME =
"/foo_service";
110 _node = rclcpp::Node::make_shared(
"node");
112 SERVICE_NAME, [&](std::shared_ptr<std_srvs::srv::SetBool::Request> req,
113 std::shared_ptr<std_srvs::srv::SetBool::Response> res) {
114 res->message =
"hello";
115 res->success = req->data;
117 executor.add_node(
_node);
121 executor.remove_node(
_node);
126 std::shared_ptr<foxglove::Client<websocketpp::config::asio_client>>
_wsClient;
131 public testing::WithParamInterface<std::pair<std::string, std::vector<uint8_t>>> {};
139 _node = rclcpp::Node::make_shared(
"node");
141 _node->create_publisher<std_msgs::msg::String>(
TOPIC_NAME, rclcpp::SystemDefaultsQoS());
154 std::shared_ptr<rclcpp::SerializedMessage>
serializeMsg(
const T* msg) {
155 using rosidl_typesupport_cpp::get_message_type_support_handle;
156 auto typeSupportHdl = get_message_type_support_handle<T>();
157 auto result = std::make_shared<rclcpp::SerializedMessage>();
158 rmw_ret_t ret = rmw_serialize(msg, typeSupportHdl, &result->get_rcl_serialized_message());
159 EXPECT_EQ(ret, RMW_RET_OK);
165 using rosidl_typesupport_cpp::get_message_type_support_handle;
166 auto typeSupportHdl = get_message_type_support_handle<T>();
167 auto result = std::make_shared<T>();
168 rmw_ret_t ret = rmw_deserialize(msg, typeSupportHdl, result.get());
169 EXPECT_EQ(ret, RMW_RET_OK);
173 TEST(SmokeTest, testConnection) {
178 TEST(SmokeTest, testSubscription) {
180 const std::string topic_name =
"/pub_topic";
181 std_msgs::msg::String rosMsg;
182 rosMsg.data =
"hello world";
184 auto node = rclcpp::Node::make_shared(
"tester");
185 rclcpp::QoS qos = rclcpp::QoS{rclcpp::KeepLast(1lu)};
187 qos.transient_local();
188 auto pub = node->create_publisher<std_msgs::msg::String>(topic_name, qos);
189 pub->publish(rosMsg);
192 const auto clientCount = 3;
193 for (
auto i = 0; i < clientCount; ++i) {
195 auto client = std::make_shared<foxglove::Client<websocketpp::config::asio_client>>();
197 ASSERT_EQ(std::future_status::ready, client->connect(
URI).wait_for(
ONE_SECOND));
198 ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(
ONE_SECOND));
204 client->subscribe({{subscriptionId, channel.
id}});
205 ASSERT_EQ(std::future_status::ready, msgFuture.wait_for(
ONE_SECOND));
206 const auto msgData = msgFuture.get();
208 EXPECT_EQ(0, std::memcmp(
HELLO_WORLD_CDR, msgData.data(), msgData.size()));
211 client->unsubscribe({subscriptionId});
215 TEST(SmokeTest, testSubscriptionParallel) {
217 const std::string topic_name =
"/pub_topic";
218 std_msgs::msg::String rosMsg;
219 rosMsg.data =
"hello world";
221 auto node = rclcpp::Node::make_shared(
"tester");
222 rclcpp::QoS qos = rclcpp::QoS{rclcpp::KeepLast(1lu)};
224 qos.transient_local();
225 auto pub = node->create_publisher<std_msgs::msg::String>(topic_name, qos);
226 pub->publish(rosMsg);
231 std::make_shared<foxglove::Client<websocketpp::config::asio_client>>(),
233 std::make_shared<foxglove::Client<websocketpp::config::asio_client>>(),
236 std::vector<std::future<std::vector<uint8_t>>> futures;
237 for (
auto client : clients) {
241 for (
auto client : clients) {
243 ASSERT_EQ(std::future_status::ready, client->connect(
URI).wait_for(
ONE_SECOND));
244 ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(
ONE_SECOND));
246 client->subscribe({{subscriptionId, channel.
id}});
249 for (
auto& future : futures) {
250 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
251 auto msgData = future.get();
253 EXPECT_EQ(0, std::memcmp(
HELLO_WORLD_CDR, msgData.data(), msgData.size()));
256 for (
auto client : clients) {
257 client->unsubscribe({subscriptionId});
262 const auto& [encoding, message] = GetParam();
266 advertisement.
topic =
"/foo";
268 advertisement.
schemaName =
"std_msgs/msg/String";
273 std::promise<std::string> msgPromise;
274 auto msgFuture = msgPromise.get_future();
275 auto node = rclcpp::Node::make_shared(
"tester");
276 auto sub = node->create_subscription<std_msgs::msg::String>(
277 advertisement.
topic, 10, [&msgPromise](std::shared_ptr<const std_msgs::msg::String> msg) {
278 msgPromise.set_value(msg->data);
280 this->executor.add_node(node);
283 auto client = std::make_shared<foxglove::Client<websocketpp::config::asio_client>>();
284 ASSERT_EQ(std::future_status::ready, client->connect(
URI).wait_for(
ONE_SECOND));
285 client->advertise({advertisement});
289 ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(
ONE_SECOND));
292 client->publish(advertisement.
channelId, message.data(), message.size());
293 client->unadvertise({advertisement.
channelId});
296 ASSERT_EQ(std::future_status::ready, msgFuture.wait_for(
DEFAULT_TIMEOUT));
297 this->executor.remove_node(node);
298 EXPECT_EQ(
"hello world", msgFuture.get());
303 testing::Values(std::make_pair(
"cdr", std::vector<uint8_t>(
HELLO_WORLD_CDR,
308 testing::Values(std::make_pair(
"json", std::vector<uint8_t>(
HELLO_WORLD_JSON,
312 const auto& [encoding, message] = GetParam();
316 advertisement.
topic = TOPIC_NAME;
318 advertisement.
schemaName =
"std_msgs/msg/String";
319 advertisement.
schema = {};
322 std::promise<std::string> msgPromise;
323 auto msgFuture = msgPromise.get_future();
324 auto node = rclcpp::Node::make_shared(
"tester");
325 auto sub = node->create_subscription<std_msgs::msg::String>(
326 advertisement.
topic, 10, [&msgPromise](std::shared_ptr<const std_msgs::msg::String> msg) {
327 msgPromise.set_value(msg->data);
329 rclcpp::executors::SingleThreadedExecutor executor;
330 executor.add_node(node);
333 auto client = std::make_shared<foxglove::Client<websocketpp::config::asio_client>>();
334 ASSERT_EQ(std::future_status::ready, client->connect(
URI).wait_for(
ONE_SECOND));
335 client->advertise({advertisement});
339 ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(
ONE_SECOND));
342 client->publish(advertisement.
channelId, message.data(), message.size());
343 client->unadvertise({advertisement.
channelId});
346 const auto ret = executor.spin_until_future_complete(msgFuture,
ONE_SECOND);
347 ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
348 EXPECT_EQ(
"hello world", msgFuture.get());
353 testing::Values(std::make_pair(
"cdr", std::vector<uint8_t>(
HELLO_WORLD_CDR,
358 testing::Values(std::make_pair(
"json", std::vector<uint8_t>(
HELLO_WORLD_JSON,
362 const std::string requestId =
"req-testGetAllParams";
364 _wsClient->getParameters({}, requestId);
365 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
366 std::vector<foxglove::Parameter> params = future.get();
368 EXPECT_GE(params.size(), 2UL);
372 const std::string requestId =
"req-testGetNonExistingParameters";
374 _wsClient->getParameters(
375 {
"/foo_1.non_existing_parameter",
"/foo_2.non_existing.nested_parameter"}, requestId);
376 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
377 std::vector<foxglove::Parameter> params = future.get();
379 EXPECT_TRUE(params.empty());
383 const auto p1 = NODE_1_NAME +
"." + PARAM_1_NAME;
384 const auto p2 = NODE_2_NAME +
"." + PARAM_2_NAME;
386 const std::string requestId =
"req-testGetParameters";
388 _wsClient->getParameters({p1, p2}, requestId);
389 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
390 std::vector<foxglove::Parameter> params = future.get();
392 EXPECT_EQ(2UL, params.size());
393 auto p1Iter = std::find_if(params.begin(), params.end(), [&p1](
const auto&
param) {
394 return param.getName() == p1;
396 auto p2Iter = std::find_if(params.begin(), params.end(), [&p2](
const auto&
param) {
397 return param.getName() == p2;
399 ASSERT_NE(p1Iter, params.end());
400 EXPECT_EQ(PARAM_1_DEFAULT_VALUE, p1Iter->getValue().getValue<PARAM_1_TYPE>());
401 ASSERT_NE(p2Iter, params.end());
403 std::vector<int64_t> int_array_val;
404 const auto array_params = p2Iter->getValue().getValue<std::vector<foxglove::ParameterValue>>();
405 for (
const auto& paramValue : array_params) {
406 int_array_val.push_back(paramValue.getValue<int64_t>());
408 EXPECT_EQ(int_array_val, PARAM_2_DEFAULT_VALUE);
412 const auto p1 = NODE_1_NAME +
"." + PARAM_1_NAME;
413 const auto p2 = NODE_2_NAME +
"." + PARAM_2_NAME;
414 const PARAM_1_TYPE newP1value =
"world";
415 const std::vector<foxglove::ParameterValue> newP2value = {4, 5, 6};
417 const std::vector<foxglove::Parameter> parameters = {
422 _wsClient->setParameters(parameters);
423 const std::string requestId =
"req-testSetParameters";
425 _wsClient->getParameters({p1, p2}, requestId);
426 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
427 std::vector<foxglove::Parameter> params = future.get();
429 EXPECT_EQ(2UL, params.size());
430 auto p1Iter = std::find_if(params.begin(), params.end(), [&p1](
const auto&
param) {
431 return param.getName() == p1;
433 auto p2Iter = std::find_if(params.begin(), params.end(), [&p2](
const auto&
param) {
434 return param.getName() == p2;
436 ASSERT_NE(p1Iter, params.end());
437 EXPECT_EQ(newP1value, p1Iter->getValue().getValue<PARAM_1_TYPE>());
438 ASSERT_NE(p2Iter, params.end());
440 std::vector<int64_t> int_array_val;
441 const auto array_params = p2Iter->getValue().getValue<std::vector<foxglove::ParameterValue>>();
442 for (
const auto& paramValue : array_params) {
443 int_array_val.push_back(paramValue.getValue<int64_t>());
445 const std::vector<int64_t> expected_value = {4, 5, 6};
446 EXPECT_EQ(int_array_val, expected_value);
450 const auto p1 = NODE_1_NAME +
"." + PARAM_1_NAME;
451 const PARAM_1_TYPE newP1value =
"world";
452 const std::vector<foxglove::Parameter> parameters = {
456 const std::string requestId =
"req-testSetParameters";
458 _wsClient->setParameters(parameters, requestId);
459 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
460 std::vector<foxglove::Parameter> params = future.get();
462 EXPECT_EQ(1UL, params.size());
466 const auto floatParamName = NODE_2_NAME +
"." + PARAM_3_NAME;
467 const auto floatArrayParamName = NODE_2_NAME +
"." + PARAM_4_NAME;
468 const int64_t floatParamVal = 10;
469 const std::vector<int64_t> floatArrayParamVal = {3, 2, 1};
470 const std::string requestId =
"req-testSetFloatParametersWithIntegers";
472 const nlohmann::json::array_t parameters = {
473 {{
"name", floatParamName}, {
"value", floatParamVal}, {
"type",
"float64"}},
474 {{
"name", floatArrayParamName}, {
"value", floatArrayParamVal}, {
"type",
"float64_array"}},
477 nlohmann::json{{
"op",
"setParameters"}, {
"id", requestId}, {
"parameters", parameters}}.dump());
478 ASSERT_EQ(std::future_status::ready, future.wait_for(
ONE_SECOND));
479 std::vector<foxglove::Parameter> params = future.get();
483 std::find_if(params.begin(), params.end(), [floatParamName](
const foxglove::Parameter& p) {
484 return p.getName() == floatParamName;
486 ASSERT_NE(
param, params.end());
488 EXPECT_NEAR(
param->getValue().getValue<
double>(),
static_cast<double>(floatParamVal), 1e-9);
491 const auto param = std::find_if(params.begin(), params.end(),
493 return p.getName() == floatArrayParamName;
495 ASSERT_NE(
param, params.end());
497 const auto paramValue =
param->getValue().getValue<std::vector<foxglove::ParameterValue>>();
498 ASSERT_EQ(paramValue.size(), floatArrayParamVal.size());
499 for (
size_t i = 0; i < paramValue.size(); ++i) {
500 EXPECT_NEAR(paramValue[i].getValue<double>(),
static_cast<double>(floatArrayParamVal[i]),
507 const auto p1 = NODE_1_NAME +
"." + DELETABLE_PARAM_NAME;
508 const std::vector<foxglove::Parameter> parameters = {
512 const std::string requestId =
"req-testUnsetParameter";
514 _wsClient->setParameters(parameters, requestId);
515 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
516 std::vector<foxglove::Parameter> params = future.get();
518 EXPECT_EQ(0UL, params.size());
522 const auto p1 = NODE_1_NAME +
"." + PARAM_1_NAME;
524 _wsClient->subscribeParameterUpdates({p1});
527 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
528 std::vector<foxglove::Parameter> params = future.get();
530 ASSERT_EQ(1UL, params.size());
531 EXPECT_EQ(params.front().getName(), p1);
533 _wsClient->unsubscribeParameterUpdates({p1});
537 ASSERT_EQ(std::future_status::timeout, future.wait_for(
ONE_SECOND));
543 std::make_shared<foxglove::Client<websocketpp::config::asio_client>>(),
545 std::make_shared<foxglove::Client<websocketpp::config::asio_client>>(),
548 std::vector<std::future<std::vector<foxglove::Parameter>>> futures;
549 for (
auto client : clients) {
551 std::async(std::launch::async, [client]() -> std::vector<foxglove::Parameter> {
553 const std::string requestId =
"req-123";
555 client->getParameters({}, requestId);
557 if (future.valid()) {
565 for (
auto& future : futures) {
566 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
567 std::vector<foxglove::Parameter> parameters;
568 EXPECT_NO_THROW(parameters = future.get());
569 EXPECT_GE(parameters.size(), 2UL);
574 auto client = std::make_shared<foxglove::Client<websocketpp::config::asio_client>>();
576 ASSERT_EQ(std::future_status::ready, client->connect(
URI).wait_for(
ONE_SECOND));
577 ASSERT_EQ(std::future_status::ready, serviceFuture.wait_for(
DEFAULT_TIMEOUT));
580 EXPECT_EQ(service.
name, SERVICE_NAME);
581 EXPECT_EQ(service.
type,
"std_srvs/srv/SetBool");
582 EXPECT_EQ(service.
requestSchema,
"bool data # e.g. for hardware enabling / disabling");
584 "bool success # indicate successful run of triggered service\nstring message # "
585 "informational, e.g. for error messages");
591 std::make_shared<foxglove::Client<websocketpp::config::asio_client>>(),
593 std::make_shared<foxglove::Client<websocketpp::config::asio_client>>(),
597 for (
auto client : clients) {
598 ASSERT_EQ(std::future_status::ready, client->connect(
URI).wait_for(
ONE_SECOND));
600 ASSERT_EQ(std::future_status::ready, serviceFuture.wait_for(
DEFAULT_TIMEOUT));
603 std_srvs::srv::SetBool::Request requestMsg;
604 requestMsg.data =
true;
605 const auto serializedRequest =
serializeMsg(&requestMsg);
606 const auto& serRequestMsg = serializedRequest->get_rcl_serialized_message();
612 request.
data.resize(serRequestMsg.buffer_length);
613 std::memcpy(request.
data.data(), serRequestMsg.buffer, serRequestMsg.buffer_length);
615 std::vector<std::future<foxglove::ServiceResponse>> futures;
616 for (
auto client : clients) {
618 client->sendServiceRequest(request);
621 for (
auto& future : futures) {
622 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
624 EXPECT_NO_THROW(
response = future.get());
629 rclcpp::SerializedMessage serializedResponseMsg(
response.data.size());
630 auto& serMsg = serializedResponseMsg.get_rcl_serialized_message();
632 serMsg.buffer_length =
response.data.size();
633 const auto resMsg = deserializeMsg<std_srvs::srv::SetBool::Response>(&serMsg);
635 EXPECT_EQ(resMsg->message,
"hello");
636 EXPECT_EQ(resMsg->success, requestMsg.data);
641 auto client = std::make_shared<foxglove::Client<websocketpp::config::asio_client>>();
642 ASSERT_EQ(std::future_status::ready, client->connect(
URI).wait_for(std::chrono::seconds(5)));
644 std::promise<nlohmann::json> promise;
645 auto serviceFailureFuture = promise.get_future();
646 client->setTextMessageHandler([&promise](
const std::string& payload)
mutable {
647 const auto msg = nlohmann::json::parse(payload);
648 if (msg[
"op"].get<std::string>() ==
"serviceCallFailure") {
649 promise.set_value(msg);
658 client->sendServiceRequest(request);
660 ASSERT_EQ(std::future_status::ready, serviceFailureFuture.wait_for(std::chrono::seconds(5)));
661 const auto failureMsg = serviceFailureFuture.get();
662 EXPECT_EQ(failureMsg[
"serviceId"].get<foxglove::ServiceId>(), request.
serviceId);
663 EXPECT_EQ(failureMsg[
"callId"].get<uint32_t>(), request.
callId);
666 TEST(SmokeTest, receiveMessagesOfMultipleTransientLocalPublishers) {
667 const std::string topicName =
"/latched";
668 auto node = rclcpp::Node::make_shared(
"node");
669 rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(1));
670 qos.transient_local();
673 rclcpp::executors::SingleThreadedExecutor executor;
674 executor.add_node(node);
675 auto spinnerThread = std::thread([&executor]() {
679 constexpr
size_t nPubs = 15;
680 std::vector<rclcpp::Publisher<std_msgs::msg::String>::SharedPtr> pubs;
681 for (
size_t i = 0; i < nPubs; ++i) {
682 auto pub = pubs.emplace_back(node->create_publisher<std_msgs::msg::String>(topicName, qos));
683 std_msgs::msg::String msg;
689 auto client = std::make_shared<foxglove::Client<websocketpp::config::asio_client>>();
691 ASSERT_EQ(std::future_status::ready, client->connect(
URI).wait_for(
ONE_SECOND));
692 ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(
ONE_SECOND));
697 std::promise<void> promise;
698 std::atomic<size_t> nReceivedMessages = 0;
699 client->setBinaryMessageHandler([&promise, &nReceivedMessages](
const uint8_t*,
size_t) {
700 if (++nReceivedMessages == nPubs) {
706 client->subscribe({{subscriptionId, channel.
id}});
707 EXPECT_EQ(std::future_status::ready, promise.get_future().wait_for(
DEFAULT_TIMEOUT));
708 EXPECT_EQ(nReceivedMessages, nPubs);
709 client->unsubscribe({subscriptionId});
712 executor.remove_node(node);
714 spinnerThread.join();
717 TEST(FetchAssetTest, fetchExistingAsset) {
718 auto wsClient = std::make_shared<foxglove::Client<websocketpp::config::asio_client>>();
719 EXPECT_EQ(std::future_status::ready, wsClient->connect(
URI).wait_for(
DEFAULT_TIMEOUT));
721 const auto millisSinceEpoch = std::chrono::duration_cast<std::chrono::milliseconds>(
723 const auto tmpFilePath =
724 std::filesystem::temp_directory_path() / std::to_string(millisSinceEpoch.count());
725 constexpr
char content[] =
"Hello, world";
726 FILE* tmpAssetFile = std::fopen(tmpFilePath.c_str(),
"w");
727 std::fputs(content, tmpAssetFile);
728 std::fclose(tmpAssetFile);
730 const std::string uri = std::string(
"file://") + tmpFilePath.string();
731 const uint32_t requestId = 123;
734 wsClient->fetchAsset(uri, requestId);
735 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
738 EXPECT_EQ(
response.requestId, requestId);
741 ASSERT_EQ(
response.data.size() + 1ul,
sizeof(content));
742 EXPECT_EQ(0, std::memcmp(content,
response.data.data(),
response.data.size()));
743 std::remove(tmpFilePath.c_str());
746 TEST(FetchAssetTest, fetchNonExistingAsset) {
747 auto wsClient = std::make_shared<foxglove::Client<websocketpp::config::asio_client>>();
748 EXPECT_EQ(std::future_status::ready, wsClient->connect(
URI).wait_for(
DEFAULT_TIMEOUT));
750 const std::string assetId =
"file:///foo/bar";
751 const uint32_t requestId = 456;
754 wsClient->fetchAsset(assetId, requestId);
755 ASSERT_EQ(std::future_status::ready, future.wait_for(
DEFAULT_TIMEOUT));
758 EXPECT_EQ(
response.requestId, requestId);
760 EXPECT_FALSE(
response.errorMessage.empty());
764 int main(
int argc,
char** argv) {
765 testing::InitGoogleTest(&argc, argv);
766 rclcpp::init(argc, argv);
768 rclcpp::executors::SingleThreadedExecutor executor;
769 rclcpp::NodeOptions nodeOptions;
771 nodeOptions.append_parameter_override(
"asset_uri_allowlist",
772 std::vector<std::string>({
"file://.*"}));
774 executor.add_node(node.get_node_base_interface());
776 std::thread spinnerThread([&executor]() {
780 const auto testResult = RUN_ALL_TESTS();
783 spinnerThread.join();
784 executor.remove_node(node.get_node_base_interface());