Refs #5773. Add workaround fix.
This commit is contained in:
julianbermudez
parent
c59ead14c6
commit
e8426e9de4
|
|
@ -263,6 +263,7 @@ endif()
|
|||
if(UCLIENT_BUILD_EXAMPLES)
|
||||
add_subdirectory(examples/PublishHelloWorld)
|
||||
add_subdirectory(examples/SubscribeHelloWorld)
|
||||
add_subdirectory(examples/Imu)
|
||||
add_subdirectory(examples/ShapesDemo)
|
||||
add_subdirectory(examples/Deployment)
|
||||
add_subdirectory(examples/Discovery)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,45 @@
|
|||
|
||||
cmake_minimum_required(VERSION 2.8.12)
|
||||
if (${CMAKE_VERSION} VERSION_GREATER 3.0)
|
||||
cmake_policy(SET CMP0048 NEW)
|
||||
endif()
|
||||
|
||||
project(client_examples)
|
||||
|
||||
#find_package(microcdr REQUIRED)
|
||||
#find_package(microxrcedds_client REQUIRED)
|
||||
|
||||
set(SRCS
|
||||
Imu.c
|
||||
)
|
||||
|
||||
# Imu Subscriber
|
||||
add_executable(ImuSubscriber ImuSubscriber.c ${SRCS})
|
||||
target_link_libraries(ImuSubscriber microxrcedds_client)
|
||||
set_target_properties(ImuSubscriber PROPERTIES
|
||||
C_STANDARD
|
||||
99
|
||||
C_STANDARD_REQUIRED
|
||||
YES
|
||||
)
|
||||
target_compile_options(ImuSubscriber
|
||||
PRIVATE
|
||||
$<$<C_COMPILER_ID:MSVC>:/W4 /wd4700 /wd4996 /wd4820 /wd4255 /wd4668>
|
||||
$<$<C_COMPILER_ID:GNU>:-pedantic -fstrict-aliasing -Wall -Wcast-align -Wshadow>
|
||||
)
|
||||
|
||||
# Imu Publisher
|
||||
add_executable(ImuPublisher ImuPublisher.c ${SRCS})
|
||||
target_link_libraries(ImuPublisher microxrcedds_client)
|
||||
set_target_properties(ImuPublisher PROPERTIES
|
||||
C_STANDARD
|
||||
99
|
||||
C_STANDARD_REQUIRED
|
||||
YES
|
||||
)
|
||||
target_compile_options(ImuPublisher
|
||||
PRIVATE
|
||||
$<$<C_COMPILER_ID:MSVC>:/W4 /wd4700 /wd4996 /wd4820 /wd4255 /wd4668>
|
||||
$<$<C_COMPILER_ID:GNU>:-pedantic -fstrict-aliasing -Wall -Wcast-align -Wshadow>
|
||||
)
|
||||
|
||||
|
|
@ -0,0 +1,149 @@
|
|||
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*!
|
||||
* @file Imu.c
|
||||
* This source file contains the definition of the described types in the IDL file.
|
||||
*
|
||||
* This file was generated by the tool gen.
|
||||
*/
|
||||
|
||||
#include "Imu.h"
|
||||
|
||||
#include <ucdr/microcdr.h>
|
||||
#include <string.h>
|
||||
|
||||
bool SensorMsgImu_serialize_topic(ucdrBuffer* writer, const SensorMsgImu* topic)
|
||||
{
|
||||
(void) ucdr_serialize_int32_t(writer, topic->sec);
|
||||
|
||||
(void) ucdr_serialize_uint32_t(writer, topic->index);
|
||||
|
||||
(void) ucdr_serialize_string(writer, topic->frame_id);
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->orientation_x);
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->orientation_y);
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->orientation_z);
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->orientation_w);
|
||||
|
||||
(void) ucdr_serialize_array_double(writer, topic->orientation_covariance, sizeof(topic->orientation_covariance) / sizeof(double));
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->angular_velocity_x);
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->angular_velocity_y);
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->angular_velocity_z);
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->angular_velocity_w);
|
||||
|
||||
(void) ucdr_serialize_array_double(writer, topic->angular_velocity_covariance, sizeof(topic->angular_velocity_covariance) / sizeof(double));
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->linear_acceleration_x);
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->linear_acceleration_y);
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->linear_acceleration_z);
|
||||
|
||||
(void) ucdr_serialize_double(writer, topic->linear_acceleration_w);
|
||||
|
||||
(void) ucdr_serialize_array_double(writer, topic->linear_acceleration_covariance, sizeof(topic->linear_acceleration_covariance) / sizeof(double));
|
||||
|
||||
return !writer->error;
|
||||
}
|
||||
|
||||
bool SensorMsgImu_deserialize_topic(ucdrBuffer* reader, SensorMsgImu* topic)
|
||||
{
|
||||
(void) ucdr_deserialize_int32_t(reader, &topic->sec);
|
||||
|
||||
(void) ucdr_deserialize_uint32_t(reader, &topic->index);
|
||||
|
||||
(void) ucdr_deserialize_string(reader, topic->frame_id, 255);
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->orientation_x);
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->orientation_y);
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->orientation_z);
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->orientation_w);
|
||||
|
||||
(void) ucdr_deserialize_array_double(reader, topic->orientation_covariance, sizeof(topic->orientation_covariance) / sizeof(double));
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->angular_velocity_x);
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->angular_velocity_y);
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->angular_velocity_z);
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->angular_velocity_w);
|
||||
|
||||
(void) ucdr_deserialize_array_double(reader, topic->angular_velocity_covariance, sizeof(topic->angular_velocity_covariance) / sizeof(double));
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->linear_acceleration_x);
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->linear_acceleration_y);
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->linear_acceleration_z);
|
||||
|
||||
(void) ucdr_deserialize_double(reader, &topic->linear_acceleration_w);
|
||||
|
||||
(void) ucdr_deserialize_array_double(reader, topic->linear_acceleration_covariance, sizeof(topic->linear_acceleration_covariance) / sizeof(double));
|
||||
|
||||
return !reader->error;
|
||||
}
|
||||
|
||||
uint32_t SensorMsgImu_size_of_topic(const SensorMsgImu* topic, uint32_t size)
|
||||
{
|
||||
uint32_t previousSize = size;
|
||||
size += ucdr_alignment(size, 4) + 4;
|
||||
|
||||
size += ucdr_alignment(size, 4) + 4;
|
||||
|
||||
size += ucdr_alignment(size, 4) + 4 + (uint32_t)strlen(topic->frame_id) + 1;
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + sizeof(topic->orientation_covariance);
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + sizeof(topic->angular_velocity_covariance);
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + 8;
|
||||
|
||||
size += ucdr_alignment(size, 8) + sizeof(topic->linear_acceleration_covariance);
|
||||
|
||||
return size - previousSize;
|
||||
}
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*!
|
||||
* @file Imu.h
|
||||
* This header file contains the declaration of the described types in the IDL file.
|
||||
*
|
||||
* This file was generated by the tool gen.
|
||||
*/
|
||||
|
||||
#ifndef _Imu_H_
|
||||
#define _Imu_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*!
|
||||
* @brief This struct represents the structure SensorMsgImu defined by the user in the IDL file.
|
||||
* @ingroup IMU
|
||||
*/
|
||||
typedef struct SensorMsgImu
|
||||
{
|
||||
int32_t sec;
|
||||
uint32_t index;
|
||||
char frame_id[255];
|
||||
|
||||
double orientation_x;
|
||||
double orientation_y;
|
||||
double orientation_z;
|
||||
double orientation_w;
|
||||
double orientation_covariance[9];
|
||||
double angular_velocity_x;
|
||||
double angular_velocity_y;
|
||||
double angular_velocity_z;
|
||||
double angular_velocity_w;
|
||||
double angular_velocity_covariance[9];
|
||||
double linear_acceleration_x;
|
||||
double linear_acceleration_y;
|
||||
double linear_acceleration_z;
|
||||
double linear_acceleration_w;
|
||||
double linear_acceleration_covariance[9];
|
||||
} SensorMsgImu;
|
||||
|
||||
struct ucdrBuffer;
|
||||
|
||||
bool SensorMsgImu_serialize_topic(struct ucdrBuffer* writer, const SensorMsgImu* topic);
|
||||
bool SensorMsgImu_deserialize_topic(struct ucdrBuffer* reader, SensorMsgImu* topic);
|
||||
uint32_t SensorMsgImu_size_of_topic(const SensorMsgImu* topic, uint32_t size);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // _Imu_H_
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
struct SensorMsgImu
|
||||
{
|
||||
// header
|
||||
long sec;
|
||||
unsigned long nsec;
|
||||
string frame_id;
|
||||
|
||||
// orientation
|
||||
double orientation_x;
|
||||
double orientation_y;
|
||||
double orientation_z;
|
||||
double orientation_w;
|
||||
|
||||
// orientation covariance
|
||||
double orientation_covariance[9];
|
||||
|
||||
// angular velocity
|
||||
double angular_velocity_x;
|
||||
double angular_velocity_y;
|
||||
double angular_velocity_z;
|
||||
double angular_velocity_w;
|
||||
|
||||
// angular velocity covariance
|
||||
double angular_velocity_covariance[9];
|
||||
|
||||
// linear acceleration
|
||||
double linear_acceleration_x;
|
||||
double linear_acceleration_y;
|
||||
double linear_acceleration_z;
|
||||
double linear_acceleration_w;
|
||||
|
||||
// linear acceleration covariance
|
||||
double linear_acceleration_covariance[9];
|
||||
};
|
||||
|
|
@ -0,0 +1,173 @@
|
|||
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*!
|
||||
* @file ImuPublisher.c
|
||||
* Publisher example.
|
||||
*
|
||||
* This file was generated by the tool microxrceddsgen.
|
||||
*/
|
||||
|
||||
#include "Imu.h"
|
||||
|
||||
#include <uxr/client/client.h>
|
||||
#include <ucdr/microcdr.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#define STREAM_HISTORY 8
|
||||
#define BUFFER_SIZE UXR_CONFIG_UDP_TRANSPORT_MTU * STREAM_HISTORY
|
||||
|
||||
|
||||
const char* participant_xml = "<dds>"
|
||||
"<participant>"
|
||||
"<rtps>"
|
||||
"<name>default_xrce_participant</name>"
|
||||
"</rtps>"
|
||||
"</participant>"
|
||||
"</dds>";
|
||||
const char* topic_xml = "<dds>"
|
||||
"<topic>"
|
||||
"<name>ImuPubSubTopic</name>"
|
||||
"<dataType>SensorMsgImu</dataType>"
|
||||
"</topic>"
|
||||
"</dds>";
|
||||
const char* publisher_xml = "";
|
||||
const char* datawriter_xml = "<dds>"
|
||||
"<data_writer>"
|
||||
"<topic>"
|
||||
"<kind>NO_KEY</kind>"
|
||||
"<name>ImuPubSubTopic</name>"
|
||||
"<dataType>SensorMsgImu</dataType>"
|
||||
"</topic>"
|
||||
"</data_writer>"
|
||||
"</dds>";
|
||||
|
||||
int main(void)
|
||||
{
|
||||
// Transport
|
||||
uxrUDPTransport transport;
|
||||
uxrUDPPlatform udp_platform;
|
||||
if(!uxr_init_udp_transport(&transport, &udp_platform, "127.0.0.1", 2018))
|
||||
{
|
||||
printf("Error at create transport.\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Session
|
||||
uxrSession session;
|
||||
uxr_init_session(&session, &transport.comm, 0xAAAABBBB);
|
||||
if(!uxr_create_session(&session))
|
||||
{
|
||||
printf("Error at create session.\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Streams
|
||||
uint8_t output_reliable_stream_buffer[BUFFER_SIZE];
|
||||
uxrStreamId reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer, BUFFER_SIZE, STREAM_HISTORY);
|
||||
|
||||
uint8_t input_reliable_stream_buffer[BUFFER_SIZE];
|
||||
uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer, BUFFER_SIZE, STREAM_HISTORY);
|
||||
|
||||
// Create entities
|
||||
uxrObjectId participant_id = uxr_object_id(0x01, UXR_PARTICIPANT_ID);
|
||||
uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, 0, participant_xml, UXR_REPLACE);
|
||||
|
||||
uxrObjectId topic_id = uxr_object_id(0x01, UXR_TOPIC_ID);
|
||||
uint16_t topic_req = uxr_buffer_create_topic_xml(&session, reliable_out, topic_id, participant_id, topic_xml, UXR_REPLACE);
|
||||
|
||||
uxrObjectId publisher_id = uxr_object_id(0x01, UXR_PUBLISHER_ID);
|
||||
uint16_t publisher_req = uxr_buffer_create_publisher_xml(&session, reliable_out, publisher_id, participant_id, publisher_xml, UXR_REPLACE);
|
||||
|
||||
uxrObjectId datawriter_id = uxr_object_id(0x01, UXR_DATAWRITER_ID);
|
||||
uint16_t datawriter_req = uxr_buffer_create_datawriter_xml(&session, reliable_out, datawriter_id, publisher_id, datawriter_xml, UXR_REPLACE);
|
||||
|
||||
// Send create entities message and wait its status
|
||||
uint8_t status[4];
|
||||
uint16_t requests[4] = {participant_req, topic_req, publisher_req, datawriter_req};
|
||||
if(!uxr_run_session_until_all_status(&session, 1000, requests, status, 4))
|
||||
{
|
||||
printf("Error at create entities: participant: %i topic: %i publisher: %i darawriter: %i\n", status[0], status[1], status[2], status[3]);
|
||||
return 1;
|
||||
}
|
||||
|
||||
SensorMsgImu topic = {0};
|
||||
topic.sec = 1;
|
||||
topic.index = 2;
|
||||
strcpy(topic.frame_id, "eProsima");
|
||||
|
||||
topic.orientation_x = 0.3;
|
||||
topic.orientation_y = 0.4;
|
||||
topic.orientation_z = 0.5;
|
||||
topic.orientation_w = 0.6;
|
||||
|
||||
topic.orientation_covariance[0] = 0.7;
|
||||
topic.orientation_covariance[1] = 0.8;
|
||||
topic.orientation_covariance[2] = 0.9;
|
||||
topic.orientation_covariance[3] = 1.0;
|
||||
topic.orientation_covariance[4] = 1.1;
|
||||
topic.orientation_covariance[5] = 1.2;
|
||||
topic.orientation_covariance[6] = 1.3;
|
||||
topic.orientation_covariance[7] = 1.4;
|
||||
topic.orientation_covariance[8] = 1.5;
|
||||
|
||||
topic.angular_velocity_x = 0.3 + 2.0;
|
||||
topic.angular_velocity_y = 0.4 + 2.0;
|
||||
topic.angular_velocity_z = 0.5 + 2.0;
|
||||
topic.angular_velocity_w = 0.6 + 2.0;
|
||||
|
||||
topic.angular_velocity_covariance[0] = 0.7 + 2.0;
|
||||
topic.angular_velocity_covariance[1] = 0.8 + 2.0;
|
||||
topic.angular_velocity_covariance[2] = 0.9 + 2.0;
|
||||
topic.angular_velocity_covariance[3] = 1.0 + 2.0;
|
||||
topic.angular_velocity_covariance[4] = 1.1 + 2.0;
|
||||
topic.angular_velocity_covariance[5] = 1.2 + 2.0;
|
||||
topic.angular_velocity_covariance[6] = 1.3 + 2.0;
|
||||
topic.angular_velocity_covariance[7] = 1.4 + 2.0;
|
||||
topic.angular_velocity_covariance[8] = 1.5 + 2.0;
|
||||
|
||||
topic.linear_acceleration_x = 0.3 + 4.0;
|
||||
topic.linear_acceleration_y = 0.4 + 4.0;
|
||||
topic.linear_acceleration_z = 0.5 + 4.0;
|
||||
topic.linear_acceleration_w = 0.6 + 4.0;
|
||||
|
||||
topic.linear_acceleration_covariance[0] = 0.7 + 4.0;
|
||||
topic.linear_acceleration_covariance[1] = 0.8 + 4.0;
|
||||
topic.linear_acceleration_covariance[2] = 0.9 + 4.0;
|
||||
topic.linear_acceleration_covariance[3] = 1.0 + 4.0;
|
||||
topic.linear_acceleration_covariance[4] = 1.1 + 4.0;
|
||||
topic.linear_acceleration_covariance[5] = 1.2 + 4.0;
|
||||
topic.linear_acceleration_covariance[6] = 1.3 + 4.0;
|
||||
topic.linear_acceleration_covariance[7] = 1.4 + 4.0;
|
||||
topic.linear_acceleration_covariance[8] = 1.5 + 4.0;
|
||||
|
||||
// Write topics
|
||||
while(true)
|
||||
{
|
||||
ucdrBuffer mb;
|
||||
uint32_t topic_size = SensorMsgImu_size_of_topic(&topic, 0);
|
||||
uxr_prepare_output_stream(&session, reliable_out, datawriter_id, &mb, topic_size);
|
||||
SensorMsgImu_serialize_topic(&mb, &topic);
|
||||
|
||||
//send and wait one second
|
||||
uxr_run_session_time(&session, 1000);
|
||||
}
|
||||
|
||||
// Delete resources
|
||||
uxr_delete_session(&session);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,181 @@
|
|||
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*!
|
||||
* @file ImuSubscriber.c
|
||||
* Subscriber example.
|
||||
*
|
||||
* This file was generated by the tool microxrceddsgen.
|
||||
*/
|
||||
|
||||
#include "Imu.h"
|
||||
#include <uxr/client/client.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#define STREAM_HISTORY 8
|
||||
#define BUFFER_SIZE UXR_CONFIG_UDP_TRANSPORT_MTU * STREAM_HISTORY
|
||||
|
||||
|
||||
const char* participant_xml = "<dds>"
|
||||
"<participant>"
|
||||
"<rtps>"
|
||||
"<name>default_xrce_participant</name>"
|
||||
"</rtps>"
|
||||
"</participant>"
|
||||
"</dds>";
|
||||
const char* topic_xml = "<dds>"
|
||||
"<topic>"
|
||||
"<name>ImuPubSubTopic</name>"
|
||||
"<dataType>SensorMsgImu</dataType>"
|
||||
"</topic>"
|
||||
"</dds>";
|
||||
const char* subscriber_xml = "";
|
||||
const char* datareader_xml = "<dds>"
|
||||
"<data_reader>"
|
||||
"<topic>"
|
||||
"<kind>NO_KEY</kind>"
|
||||
"<name>ImuPubSubTopic</name>"
|
||||
"<dataType>SensorMsgImu</dataType>"
|
||||
"</topic>"
|
||||
"</data_reader>"
|
||||
"</dds>";
|
||||
|
||||
void on_topic(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* mb, void* args)
|
||||
{
|
||||
(void) session; (void) object_id; (void) request_id; (void) stream_id; (void) args;
|
||||
|
||||
SensorMsgImu topic;
|
||||
SensorMsgImu_deserialize_topic(mb, &topic);
|
||||
|
||||
printf("sec: %d\n", topic.sec);
|
||||
printf("nsec: %d\n", topic.index);
|
||||
printf("frame_id: %s\n", topic.frame_id);
|
||||
|
||||
printf("orientation X: %f\n", topic.orientation_x);
|
||||
printf("orientation Y: %f\n", topic.orientation_y);
|
||||
printf("orientation Z: %f\n", topic.orientation_z);
|
||||
printf("orientation W: %f\n", topic.orientation_w);
|
||||
|
||||
printf("orientation variance: [%f], [%f], [%f], [%f], [%f], [%f], [%f], [%f], [%f]\n",
|
||||
topic.orientation_covariance[0],
|
||||
topic.orientation_covariance[1],
|
||||
topic.orientation_covariance[2],
|
||||
topic.orientation_covariance[3],
|
||||
topic.orientation_covariance[4],
|
||||
topic.orientation_covariance[5],
|
||||
topic.orientation_covariance[6],
|
||||
topic.orientation_covariance[7],
|
||||
topic.orientation_covariance[8]);
|
||||
|
||||
printf("angular velocity X: %f\n", topic.angular_velocity_x);
|
||||
printf("angular velocity Y: %f\n", topic.angular_velocity_y);
|
||||
printf("angular velocity Z: %f\n", topic.angular_velocity_z);
|
||||
printf("angular velocity W: %f\n", topic.angular_velocity_w);
|
||||
|
||||
printf("angular velocity variance: [%f], [%f], [%f], [%f], [%f], [%f], [%f], [%f], [%f]\n",
|
||||
topic.angular_velocity_covariance[0],
|
||||
topic.angular_velocity_covariance[1],
|
||||
topic.angular_velocity_covariance[2],
|
||||
topic.angular_velocity_covariance[3],
|
||||
topic.angular_velocity_covariance[4],
|
||||
topic.angular_velocity_covariance[5],
|
||||
topic.angular_velocity_covariance[6],
|
||||
topic.angular_velocity_covariance[7],
|
||||
topic.angular_velocity_covariance[8]);
|
||||
|
||||
printf("linear acceleration X: %f\n", topic.linear_acceleration_x);
|
||||
printf("linear acceleration Y: %f\n", topic.linear_acceleration_y);
|
||||
printf("linear acceleration Z: %f\n", topic.linear_acceleration_z);
|
||||
printf("linear acceleration W: %f\n", topic.linear_acceleration_w);
|
||||
|
||||
printf("linear acceleration variance: [%f], [%f], [%f], [%f], [%f], [%f], [%f], [%f], [%f]\n",
|
||||
topic.linear_acceleration_covariance[0],
|
||||
topic.linear_acceleration_covariance[1],
|
||||
topic.linear_acceleration_covariance[2],
|
||||
topic.linear_acceleration_covariance[3],
|
||||
topic.linear_acceleration_covariance[4],
|
||||
topic.linear_acceleration_covariance[5],
|
||||
topic.linear_acceleration_covariance[6],
|
||||
topic.linear_acceleration_covariance[7],
|
||||
topic.linear_acceleration_covariance[8]);
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
// Transport
|
||||
uxrUDPTransport transport;
|
||||
uxrUDPPlatform udp_platform;
|
||||
if(!uxr_init_udp_transport(&transport, &udp_platform, "127.0.0.1", 2018))
|
||||
{
|
||||
printf("Error at create transport.\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Session
|
||||
uxrSession session;
|
||||
uxr_init_session(&session, &transport.comm, 0xCCCCDDDD);
|
||||
uxr_set_topic_callback(&session, on_topic, NULL);
|
||||
if(!uxr_create_session(&session))
|
||||
{
|
||||
printf("Error at create session.\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Streams
|
||||
uint8_t output_reliable_stream_buffer[BUFFER_SIZE];
|
||||
uxrStreamId reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer, BUFFER_SIZE, STREAM_HISTORY);
|
||||
|
||||
uint8_t input_reliable_stream_buffer[BUFFER_SIZE];
|
||||
uxrStreamId reliable_in = uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer, BUFFER_SIZE, STREAM_HISTORY);
|
||||
|
||||
// Create entities
|
||||
uxrObjectId participant_id = uxr_object_id(0x01, UXR_PARTICIPANT_ID);
|
||||
uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, 0, participant_xml, UXR_REPLACE);
|
||||
|
||||
uxrObjectId topic_id = uxr_object_id(0x01, UXR_TOPIC_ID);
|
||||
uint16_t topic_req = uxr_buffer_create_topic_xml(&session, reliable_out, topic_id, participant_id, topic_xml, UXR_REPLACE);
|
||||
|
||||
uxrObjectId subscriber_id = uxr_object_id(0x01, UXR_SUBSCRIBER_ID);
|
||||
uint16_t subscriber_req = uxr_buffer_create_subscriber_xml(&session, reliable_out, subscriber_id, participant_id, subscriber_xml, UXR_REPLACE);
|
||||
|
||||
uxrObjectId datareader_id = uxr_object_id(0x01, UXR_DATAREADER_ID);
|
||||
uint16_t datareader_req = uxr_buffer_create_datareader_xml(&session, reliable_out, datareader_id, subscriber_id, datareader_xml, UXR_REPLACE);
|
||||
|
||||
// Send create entities message and wait its status
|
||||
uint8_t status[4];
|
||||
uint16_t requests[4] = {participant_req, topic_req, subscriber_req, datareader_req};
|
||||
if(!uxr_run_session_until_all_status(&session, 1000, requests, status, 4))
|
||||
{
|
||||
printf("Error at create entities: participant: %i topic: %i subscriber: %i datareader: %i\n", status[0], status[1], status[2], status[3]);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Request topics
|
||||
uxrDeliveryControl delivery_control = {0};
|
||||
delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED;
|
||||
uint16_t read_data_req = uxr_buffer_request_data(&session, reliable_out, datareader_id, reliable_in, &delivery_control);
|
||||
|
||||
// Read topics
|
||||
while(true)
|
||||
{
|
||||
uint8_t read_data_status;
|
||||
(void) uxr_run_session_until_all_status(&session, UXR_TIMEOUT_INF, &read_data_req, &read_data_status, 1);
|
||||
}
|
||||
|
||||
// Delete resources
|
||||
uxr_delete_session(&session);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -155,6 +155,7 @@ inline void read_format_data(
|
|||
{
|
||||
(void) length;
|
||||
ub->last_data_size = 8; //reset alignment (as if we were created a new ucdrBuffer)
|
||||
ub->init = ub->iterator;
|
||||
|
||||
session->on_topic(session, object_id, request_id, stream_id, ub, session->on_topic_args);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@ bool uxr_prepare_output_stream(uxrSession* session, uxrStreamId stream_id, uxrOb
|
|||
(void) uxr_serialize_WRITE_DATA_Payload_Data(ub, &payload);
|
||||
|
||||
ub->last_data_size = 8; //reset alignment (as if we were created a new ucdrBuffer)
|
||||
ub->init = ub->iterator;
|
||||
}
|
||||
|
||||
return !ub->error;
|
||||
|
|
|
|||
Loading…
Reference in New Issue