Skip to content

Commit

Permalink
WIP: creating ROS2-like publisher and subscriber to reproduce eclipse…
Browse files Browse the repository at this point in the history 
…-iceoryx#1740
  • Loading branch information
Matej Vargovcik committed Oct 27, 2022
1 parent 3cbc214 commit 9082739
Show file tree
Hide file tree
Showing 7 changed files with 187 additions and 74 deletions.
111 changes: 57 additions & 54 deletions iceoryx_examples/icedelivery/iox_publisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,14 +15,14 @@
//
// SPDX-License-Identifier: Apache-2.0

#include "topic_data.hpp"

//! [include publisher]
#include "iceoryx_posh/popo/publisher.hpp"
//! [include publisher]
#include "iceoryx_dust/posix_wrapper/signal_watcher.hpp"
#include "iceoryx_posh/runtime/posh_runtime.hpp"

#include "topic_data.hpp"

#include <iostream>

constexpr char APP_NAME[] = "iox-cpp-publisher";
Expand All @@ -37,27 +37,29 @@ int main()
iox::runtime::PoshRuntime::initRuntime(APP_NAME);

//! [create publisher]
iox::popo::Publisher<RadarObject> publisher({"Radar", "FrontLeft", "Object"});
iox::popo::Publisher<double, iceoryx_header> publisher({"DDS_CYCLONE", "std_msgs::msg::dds_::Float64_", "rt/chatter_pod"});
//! [create publisher]

double ct = 0.0;
while (!iox::posix::hasTerminationRequested())
{
++ct;
double sampleValue1 = ct + 89;
double sampleValue2 = ct + 144;
double sampleValue3 = ct + 233;
double sampleValue4 = ct + 377;

//! [API Usage #1]
// * Retrieve a typed sample from shared memory.
// * Sample can be held until ready to publish.
// * Data is default constructed during loan
publisher.loan()
.and_then([&](auto& sample) {
sample->x = sampleValue1;
sample->y = sampleValue1;
sample->z = sampleValue1;
*sample = ct;
iceoryx_header &ice_hdr = sample.getUserHeader();
ice_hdr.data_kind = SDK_DATA;
ice_hdr.data_size = sizeof(double);
ice_hdr.shm_data_state = IOX_CHUNK_CONTAINS_RAW_DATA;
// ice_hdr.guid = 0;
ice_hdr.statusinfo = 0;
ice_hdr.tstamp = dds_time();

sample.publish();
})
.or_else([](auto& error) {
Expand All @@ -67,50 +69,51 @@ int main()
//! [API Usage #1]


//! [API Usage #2]
// * Retrieve a typed sample from shared memory and construct data in-place
// * Sample can be held until ready to publish.
// * Data is constructed with the arguments provided.
publisher.loan(sampleValue2, sampleValue2, sampleValue2)
.and_then([](auto& sample) { sample.publish(); })
.or_else([](auto& error) {
// Do something with error
std::cerr << "Unable to loan sample, error: " << error << std::endl;
});
//! [API Usage #2]

//! [API Usage #3]
// * Basic copy-and-publish. Useful for smaller data types.
auto object = RadarObject(sampleValue3, sampleValue3, sampleValue3);
publisher.publishCopyOf(object).or_else([](auto& error) {
// Do something with error.
std::cerr << "Unable to publishCopyOf, error: " << error << std::endl;
});
//! [API Usage #3]

//! [API Usage #4]
// * Provide a callable that will be used to populate the loaned sample.
// * The first argument of the callable must be T* and is the location that the callable should
// write its result to.
publisher.publishResultOf(getRadarObject, ct).or_else([](auto& error) {
// Do something with error.
std::cerr << "Unable to publishResultOf, error: " << error << std::endl;
});
publisher
.publishResultOf([&sampleValue4](RadarObject* object) {
*object = RadarObject(sampleValue4, sampleValue4, sampleValue4);
})
.or_else([](auto& error) {
// Do something with error.
std::cerr << "Unable to publishResultOf, error: " << error << std::endl;
});
//! [API Usage #4]


std::cout << APP_NAME << " sent values: " << sampleValue1 << ", " << sampleValue2 << ", " << sampleValue3
<< ", " << ct << ", " << sampleValue4 << std::endl;

std::this_thread::sleep_for(std::chrono::seconds(1));
// //! [API Usage #2]
// // * Retrieve a typed sample from shared memory and construct data in-place
// // * Sample can be held until ready to publish.
// // * Data is constructed with the arguments provided.
// publisher.loan(sampleValue2, sampleValue2, sampleValue2)
// .and_then([](auto& sample) { sample.publish(); })
// .or_else([](auto& error) {
// // Do something with error
// std::cerr << "Unable to loan sample, error: " << error << std::endl;
// });
// //! [API Usage #2]

// //! [API Usage #3]
// // * Basic copy-and-publish. Useful for smaller data types.
// auto object = RadarObject(sampleValue3, sampleValue3, sampleValue3);
// publisher.publishCopyOf(object).or_else([](auto& error) {
// // Do something with error.
// std::cerr << "Unable to publishCopyOf, error: " << error << std::endl;
// });
// //! [API Usage #3]

// //! [API Usage #4]
// // * Provide a callable that will be used to populate the loaned sample.
// // * The first argument of the callable must be T* and is the location that the callable should
// // write its result to.
// publisher.publishResultOf(getRadarObject, ct).or_else([](auto& error) {
// // Do something with error.
// std::cerr << "Unable to publishResultOf, error: " << error << std::endl;
// });
// publisher
// .publishResultOf([&sampleValue4](RadarObject* object) {
// *object = RadarObject(sampleValue4, sampleValue4, sampleValue4);
// })
// .or_else([](auto& error) {
// // Do something with error.
// std::cerr << "Unable to publishResultOf, error: " << error << std::endl;
// });
// //! [API Usage #4]

// std::cout << APP_NAME << " sent values: " << sampleValue1 << ", " << sampleValue2 << ", " << sampleValue3
// << ", " << ct << ", " << sampleValue4 << std::endl;

std::cout << "Publishing: " << ct << std::endl;

std::this_thread::sleep_for(std::chrono::nanoseconds(1));
}

return 0;
Expand Down
34 changes: 29 additions & 5 deletions iceoryx_examples/icedelivery/iox_subscriber.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,14 +15,14 @@
//
// SPDX-License-Identifier: Apache-2.0

#include "topic_data.hpp"

//! [include subscriber]
#include "iceoryx_posh/popo/subscriber.hpp"
//! [include subscriber]
#include "iceoryx_dust/posix_wrapper/signal_watcher.hpp"
#include "iceoryx_posh/runtime/posh_runtime.hpp"

#include "topic_data.hpp"

#include <iostream>

constexpr char APP_NAME[] = "iox-cpp-subscriber";
Expand All @@ -33,7 +33,7 @@ int main()
iox::runtime::PoshRuntime::initRuntime(APP_NAME);

//! [create subscriber]
iox::popo::Subscriber<RadarObject> subscriber({"Radar", "FrontLeft", "Object"});
iox::popo::Subscriber<double, iceoryx_header> subscriber({"DDS_CYCLONE", "std_msgs::msg::dds_::Float64_", "rt/chatter_pod"});
//! [create subscriber]

// run until interrupted by Ctrl-C
Expand All @@ -44,7 +44,31 @@ int main()
//! [sample happy path]
.and_then([](auto& sample) {
//! [print sample info]
std::cout << APP_NAME << " got value: " << sample->x << std::endl;
const iceoryx_header &ice_hdr = sample.getUserHeader();
std::cout << APP_NAME << " got value: " << *sample
<< ", stamp: "
<< ice_hdr.tstamp
<< ", statusinfo: "
<< ice_hdr.statusinfo
<< ", data_kind: "
<< (int)ice_hdr.data_kind
<< ", data_size: "
<< ice_hdr.data_size
<< ", shm_data_state: "
<< ice_hdr.shm_data_state
<< ", keyhash: "<< std::endl
<< (int)ice_hdr.keyhash.value[0]<<" "<< (int)ice_hdr.keyhash.value[1]<<" " << (int)ice_hdr.keyhash.value[2]<<" " << (int)ice_hdr.keyhash.value[3]<<" "
<< (int)ice_hdr.keyhash.value[4]<<" " << (int)ice_hdr.keyhash.value[5]<<" " << (int)ice_hdr.keyhash.value[6]<<" " << (int)ice_hdr.keyhash.value[7]<<" "
<< (int)ice_hdr.keyhash.value[8]<<" " << (int)ice_hdr.keyhash.value[9]<<" " << (int)ice_hdr.keyhash.value[10]<<" " << (int)ice_hdr.keyhash.value[11]<<" "
<< (int)ice_hdr.keyhash.value[12]<<" " << (int)ice_hdr.keyhash.value[13]<<" " << (int)ice_hdr.keyhash.value[14]<<" " << (int)ice_hdr.keyhash.value[15]
<< std::endl
<< ", guid:" << std::endl
<< (int)ice_hdr.guid.prefix.s[0]<<" "<< (int)ice_hdr.guid.prefix.s[1]<<" " << (int)ice_hdr.guid.prefix.s[2]<<" " << (int)ice_hdr.guid.prefix.s[3]<<" "
<< (int)ice_hdr.guid.prefix.s[4]<<" " << (int)ice_hdr.guid.prefix.s[5]<<" " << (int)ice_hdr.guid.prefix.s[6]<<" " << (int)ice_hdr.guid.prefix.s[7]<<" "
<< (int)ice_hdr.guid.prefix.s[8]<<" " << (int)ice_hdr.guid.prefix.s[9]<<" " << (int)ice_hdr.guid.prefix.s[10]<<" " << (int)ice_hdr.guid.prefix.s[11]<<" "
<< ice_hdr.guid.prefix.u[0]<<" " << ice_hdr.guid.prefix.u[1]<<" " << ice_hdr.guid.prefix.u[2]<<" " << ice_hdr.guid.entityid.u
<< std::endl;

//! [print sample info]
})
//! [sample happy path]
Expand All @@ -58,7 +82,7 @@ int main()
}
});

std::this_thread::sleep_for(std::chrono::milliseconds(100));
// std::this_thread::sleep_for(std::chrono::milliseconds(100));
}

return (EXIT_SUCCESS);
Expand Down
66 changes: 66 additions & 0 deletions iceoryx_examples/icedelivery/topic_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,72 @@ struct RadarObject
double y = 0.0;
double z = 0.0;
};

typedef enum {
IOX_CHUNK_UNINITIALIZED,
IOX_CHUNK_CONTAINS_RAW_DATA,
IOX_CHUNK_CONTAINS_SERIALIZED_DATA
} iox_shm_data_state_t;

typedef struct ddsi_keyhash {
unsigned char value[16];
} ddsi_keyhash_t;

typedef int64_t dds_time_t;
typedef union ddsi_guid_prefix {
unsigned char s[12];
uint32_t u[3];
} ddsi_guid_prefix_t;
typedef union ddsi_entityid {
uint32_t u;
} ddsi_entityid_t;
typedef struct ddsi_guid {
ddsi_guid_prefix_t prefix;
ddsi_entityid_t entityid;
} ddsi_guid_t;

struct iceoryx_header {
struct ddsi_guid guid;
dds_time_t tstamp;
uint32_t statusinfo;
uint32_t data_size;
unsigned char data_kind;
ddsi_keyhash_t keyhash;
iox_shm_data_state_t shm_data_state;
};
enum ddsi_serdata_kind {
SDK_EMPTY,
SDK_KEY,
SDK_DATA
};
//! [topic data]

/*
* Copyright(c) 2006 to 2018 ADLINK Technology Limited and others
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License v. 2.0 which is available at
* http://www.eclipse.org/legal/epl-2.0, or the Eclipse Distribution License
* v. 1.0 which is available at
* http://www.eclipse.org/org/documents/edl-v10.php.
*
* SPDX-License-Identifier: EPL-2.0 OR BSD-3-Clause
*/
#include <assert.h>
#include <errno.h>
#include <stdint.h>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>

#define CLOCK_REALTIME 0
#define DDS_NSECS_IN_SEC INT64_C(1000000000)

dds_time_t dds_time(void)
{
struct timespec ts;
(void)clock_gettime(CLOCK_REALTIME, &ts);
return (ts.tv_sec * DDS_NSECS_IN_SEC) + ts.tv_nsec;
}

#endif // IOX_EXAMPLES_ICEDELIVERY_TOPIC_DATA_HPP
12 changes: 5 additions & 7 deletions iceoryx_examples/icedelivery_in_c/ice_c_publisher.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ void sending(void)
options.historyCapacity = 10U;
options.nodeName = "iox-c-publisher-node";
iox_pub_storage_t publisherStorage;
iox_pub_t publisher = iox_pub_init(&publisherStorage, "Radar", "FrontLeft", "Object", &options);
iox_pub_t publisher = iox_pub_init(&publisherStorage, "DDS_CYCLONE", "std_msgs::msg::dds_::Float64_", "rt/chatter_pod", &options);
//! [create publisher port]

//! [send and print number]
Expand All @@ -56,13 +56,11 @@ void sending(void)
while (!killswitch)
{
void* userPayload = NULL;
if (AllocationResult_SUCCESS == iox_pub_loan_chunk(publisher, &userPayload, sizeof(struct RadarObject)))
if (AllocationResult_SUCCESS == iox_pub_loan_chunk(publisher, &userPayload, sizeof(double)))
{
struct RadarObject* sample = (struct RadarObject*)userPayload;
double* sample = (double*)userPayload;

sample->x = ct;
sample->y = ct;
sample->z = ct;
*sample = ct;

printf("%s sent value: %.0f\n", APP_NAME, ct);
fflush(stdout);
Expand All @@ -71,7 +69,7 @@ void sending(void)

++ct;

sleep_for(400);
// sleep_for(400);
}
else
{
Expand Down
12 changes: 6 additions & 6 deletions iceoryx_examples/icedelivery_in_c/ice_c_subscriber.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ void receiving(void)
options.nodeName = "iox-c-subscriber-node";
iox_sub_storage_t subscriberStorage;

iox_sub_t subscriber = iox_sub_init(&subscriberStorage, "Radar", "FrontLeft", "Object", &options);
iox_sub_t subscriber = iox_sub_init(&subscriberStorage, "DDS_CYCLONE", "std_msgs::msg::dds_::Float64_", "rt/chatter_pod", &options);
//! [create subscriber port]

//! [receive and print data]
Expand All @@ -66,19 +66,19 @@ void receiving(void)
// new sample every 400 ms and we check for new samples only every second
while (ChunkReceiveResult_SUCCESS == iox_sub_take_chunk(subscriber, &userPayload))
{
const struct RadarObject* sample = (const struct RadarObject*)(userPayload);
printf("%s got value: %.0f\n", APP_NAME, sample->x);
const double* sample = (const double*)(userPayload);
printf("%s got value: %.0f\n", APP_NAME, *sample);
fflush(stdout);
iox_sub_release_chunk(subscriber, userPayload);
}
printf("\n");
// printf("\n");
}
else
{
printf("Not subscribed!\n");
}

sleep_for(1000);
// sleep_for(1000);
}
//! [receive and print data]

Expand Down
Loading

0 comments on commit 9082739

Please sign in to comment.