Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix warnings #69

Merged
merged 29 commits into from
Jul 8, 2020
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 39 additions & 0 deletions .github/workflows/ci.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
name: CI RMW_MicroXRCEDDS

on:
pull_request:
branches: [ foxy ]

jobs:

rmw_microxrcedds_ci:
runs-on: ubuntu-20.04
container: pablogs9/microros_agent:foxy

steps:
- uses: actions/checkout@v2
with:
path: src/rmw-microxrcedds

- name: Download dependencies
run: |
git clone -b foxy https://github.com/eProsima/Micro-CDR src/Micro-CDR
git clone -b foxy https://github.com/eProsima/Micro-XRCE-DDS-Client src/Micro-XRCE-DDS-Client
git clone -b feature/foxy_migration https://github.com/micro-ROS/rosidl_typesupport_microxrcedds src/rosidl_typesupport_microxrcedds
touch src/rosidl_typesupport_microxrcedds/test/COLCON_IGNORE

- name: Build
run: source /opt/ros/foxy/setup.bash && colcon build

- name: Test
run: |
source /opt/ros/foxy/setup.bash && source /uros_agent/install/local_setup.bash && ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v6 &
sleep 1
source /opt/ros/foxy/setup.bash && source install/local_setup.bash
cd build/rmw_microxrcedds/test
./test-node
./test-publisher
./test-subscriber
./test-topic
./test-pubsub
./test-rmw
21 changes: 7 additions & 14 deletions rmw_microxrcedds_c/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ elseif(${RMW_UXRCE_TRANSPORT} STREQUAL "udp")
elseif(${RMW_UXRCE_IPV} STREQUAL "ipv6")
set(MICRO_XRCEDDS_IPV6 ON)
set(RMW_UXRCE_DEFAULT_UDP_IP "::1" CACHE STRING "Sets the agent IP address.")
endif ()
endif()
set(RMW_UXRCE_DEFAULT_UDP_PORT "8888" CACHE STRING "Sets the agent IP port.")
elseif(${RMW_UXRCE_TRANSPORT} STREQUAL "custom_serial")
set(MICRO_XRCEDDS_CUSTOM_SERIAL ON)
Expand All @@ -94,8 +94,7 @@ endif()

# Create source files with the define
configure_file(${PROJECT_SOURCE_DIR}/src/config.h.in
${PROJECT_BINARY_DIR}/include/rmw_microxrcedds_c/config.h
)
${PROJECT_BINARY_DIR}/include/rmw_microxrcedds_c/config.h)

set(SRCS
src/identifiers.c
Expand Down Expand Up @@ -136,8 +135,7 @@ set(SRCS
)

add_library(${PROJECT_NAME}
${SRCS}
)
${SRCS})

target_link_libraries(${PROJECT_NAME}
microcdr
Expand Down Expand Up @@ -178,8 +176,7 @@ configure_rmw_library(${PROJECT_NAME})
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
$<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/include>
)
$<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/include>)

set_target_properties(${PROJECT_NAME} PROPERTIES
C_STANDARD
Expand Down Expand Up @@ -209,21 +206,17 @@ ament_export_dependencies(

if(rosidl_typesupport_microxrcedds_c_FOUND)
ament_export_dependencies(rosidl_typesupport_microxrcedds_c)
set(implementations
"c:rosidl_typesupport_c:rosidl_typesupport_microxrcedds_c"
)
set(implementations "c:rosidl_typesupport_c:rosidl_typesupport_microxrcedds_c")
endif()
if(rosidl_typesupport_microxrcedds_cpp_FOUND)
ament_export_dependencies(rosidl_typesupport_microxrcedds_cpp)
set(implementations
${implementations}
${implementations}
"cpp:rosidl_typesupport_cpp:rosidl_typesupport_microxrcedds_cpp"
)
endif()

register_rmw_implementation(
${implementations}
)
register_rmw_implementation(${implementations})

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
Expand Down
20 changes: 12 additions & 8 deletions rmw_microxrcedds_c/include/rmw_uros/options.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,10 @@
// 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
// limitations under the License.

#ifndef RMW_MICROXRCEDDS_C__RMW_UROS_OPTIONS_H
#define RMW_MICROXRCEDDS_C__RMW_UROS_OPTIONS_H
#ifndef RMW_UROS__OPTIONS_H_
#define RMW_UROS__OPTIONS_H_

#include <rmw/ret_types.h>
#include <rmw/init_options.h>
Expand All @@ -28,7 +28,9 @@
* \return RMW_RET_OK If arguments were valid and set in rmw_init_options.
* \return RMW_RET_INVALID_ARGUMENT If rmw_init_options is not valid or unexpected arguments.
*/
rmw_ret_t rmw_uros_init_options(int argc, const char* const argv[], rmw_init_options_t* rmw_options);
rmw_ret_t rmw_uros_init_options(
int argc, const char * const argv[],
rmw_init_options_t * rmw_options);

/**
* \brief Fills rmw implementation-specific options with the given parameters.
Expand All @@ -38,7 +40,7 @@ rmw_ret_t rmw_uros_init_options(int argc, const char* const argv[], rmw_init_opt
* \return RMW_RET_OK If arguments were valid and set in rmw_init_options.
* \return RMW_RET_INVALID_ARGUMENT If rmw_init_options is not valid or unexpected arguments.
*/
rmw_ret_t rmw_uros_options_set_serial_device(const char* dev, rmw_init_options_t* rmw_options);
rmw_ret_t rmw_uros_options_set_serial_device(const char * dev, rmw_init_options_t * rmw_options);

/**
* \brief Fills rmw implementation-specific options with the given parameters.
Expand All @@ -49,7 +51,9 @@ rmw_ret_t rmw_uros_options_set_serial_device(const char* dev, rmw_init_options_t
* \return RMW_RET_OK If arguments were valid and set in rmw_init_options.
* \return RMW_RET_INVALID_ARGUMENT If rmw_init_options is not valid or unexpected arguments.
*/
rmw_ret_t rmw_uros_options_set_udp_address(const char* ip, const char* port, rmw_init_options_t* rmw_options);
rmw_ret_t rmw_uros_options_set_udp_address(
const char * ip, const char * port,
rmw_init_options_t * rmw_options);

/**
* \brief Fills rmw implementation-specific options with the given parameters.
Expand All @@ -59,7 +63,7 @@ rmw_ret_t rmw_uros_options_set_udp_address(const char* ip, const char* port, rmw
* \return RMW_RET_OK If arguments were valid and set in rmw_init_options.
* \return RMW_RET_INVALID_ARGUMENT If rmw_init_options is not valid or unexpected arguments.
*/
rmw_ret_t rmw_uros_options_set_client_key(uint32_t client_key, rmw_init_options_t* rmw_options);
rmw_ret_t rmw_uros_options_set_client_key(uint32_t client_key, rmw_init_options_t * rmw_options);


#endif // !RMW_MICROXRCEDDS_C__RMW_UROS_OPTIONS_H
#endif // RMW_UROS__OPTIONS_H_
134 changes: 80 additions & 54 deletions rmw_microxrcedds_c/src/callbacks.c
Original file line number Diff line number Diff line change
Expand Up @@ -14,11 +14,12 @@

#include "./callbacks.h"

void on_status( struct uxrSession* session,
uxrObjectId object_id,
uint16_t request_id,
uint8_t status,
void* args)
void on_status(
struct uxrSession * session,
uxrObjectId object_id,
uint16_t request_id,
uint8_t status,
void * args)
{
(void)session;
(void)object_id;
Expand All @@ -27,33 +28,42 @@ void on_status( struct uxrSession* session,
(void)args;
}

void on_topic( struct uxrSession* session,
uxrObjectId object_id,
uint16_t request_id,
uxrStreamId stream_id,
struct ucdrBuffer* ub,
uint16_t length,
void* args)
void on_topic(
struct uxrSession * session,
uxrObjectId object_id,
uint16_t request_id,
uxrStreamId stream_id,
struct ucdrBuffer * ub,
uint16_t length,
void * args)
{
(void)session;
(void)request_id;
(void)stream_id;
(void)args;

struct rmw_uxrce_mempool_item_t * subscription_item = subscription_memory.allocateditems;
while (subscription_item != NULL) {
rmw_uxrce_subscription_t * custom_subscription = (rmw_uxrce_subscription_t *)subscription_item->data;
rmw_uxrce_subscription_t * custom_subscription =
(rmw_uxrce_subscription_t *)subscription_item->data;
if ((custom_subscription->datareader_id.id == object_id.id) &&
(custom_subscription->datareader_id.type == object_id.type))
{
{
custom_subscription->micro_buffer_lenght[custom_subscription->history_write_index] = length;
ucdr_deserialize_array_uint8_t(ub, custom_subscription->micro_buffer[custom_subscription->history_write_index], length);

// TODO (Pablo): Circular overlapping buffer implemented: use qos
if (custom_subscription->micro_buffer_in_use && custom_subscription->history_write_index == custom_subscription->history_read_index){
custom_subscription->history_read_index = (custom_subscription->history_read_index + 1) % RMW_UXRCE_MAX_HISTORY;
ucdr_deserialize_array_uint8_t(
ub,
custom_subscription->micro_buffer[custom_subscription->history_write_index], length);

// TODO(pablogs9): Circular overlapping buffer implemented: use qos
if (custom_subscription->micro_buffer_in_use &&
custom_subscription->history_write_index == custom_subscription->history_read_index)
{
custom_subscription->history_read_index = (custom_subscription->history_read_index + 1) %
RMW_UXRCE_MAX_HISTORY;
}

custom_subscription->history_write_index = (custom_subscription->history_write_index + 1) % RMW_UXRCE_MAX_HISTORY;
custom_subscription->history_write_index = (custom_subscription->history_write_index + 1) %
RMW_UXRCE_MAX_HISTORY;
custom_subscription->micro_buffer_in_use = true;

break;
Expand All @@ -62,32 +72,41 @@ void on_topic( struct uxrSession* session,
}
}

void on_request(struct uxrSession* session,
uxrObjectId object_id,
uint16_t request_id,
SampleIdentity* sample_id,
struct ucdrBuffer* ub,
uint16_t length,
void* args)
void on_request(
struct uxrSession * session,
uxrObjectId object_id,
uint16_t request_id,
SampleIdentity * sample_id,
struct ucdrBuffer * ub,
uint16_t length,
void * args)
{
(void)session;
(void)object_id;
(void)args;

struct rmw_uxrce_mempool_item_t * service_item = service_memory.allocateditems;
while (service_item != NULL) {
rmw_uxrce_service_t * custom_service = (rmw_uxrce_service_t *)service_item->data;
if (custom_service->request_id == request_id){
if (custom_service->request_id == request_id) {
custom_service->micro_buffer_lenght[custom_service->history_write_index] = length;
ucdr_deserialize_array_uint8_t(ub, custom_service->micro_buffer[custom_service->history_write_index], length);
memcpy(&custom_service->sample_id[custom_service->history_write_index],
sample_id, sizeof(SampleIdentity));

// TODO (Pablo): Circular overlapping buffer implemented: use qos
if (custom_service->micro_buffer_in_use && custom_service->history_write_index == custom_service->history_read_index){
custom_service->history_read_index = (custom_service->history_read_index + 1) % RMW_UXRCE_MAX_HISTORY;
ucdr_deserialize_array_uint8_t(
ub,
custom_service->micro_buffer[custom_service->history_write_index], length);
memcpy(
&custom_service->sample_id[custom_service->history_write_index],
sample_id, sizeof(SampleIdentity));

// TODO(pablogs9): Circular overlapping buffer implemented: use qos
if (custom_service->micro_buffer_in_use &&
custom_service->history_write_index == custom_service->history_read_index)
{
custom_service->history_read_index = (custom_service->history_read_index + 1) %
RMW_UXRCE_MAX_HISTORY;
}

custom_service->history_write_index = (custom_service->history_write_index + 1) % RMW_UXRCE_MAX_HISTORY;
custom_service->history_write_index = (custom_service->history_write_index + 1) %
RMW_UXRCE_MAX_HISTORY;
custom_service->micro_buffer_in_use = true;

break;
Expand All @@ -96,36 +115,43 @@ void on_request(struct uxrSession* session,
}
}

void on_reply( struct uxrSession* session,
uxrObjectId object_id,
uint16_t request_id,
uint16_t reply_id,
struct ucdrBuffer* ub,
uint16_t length,
void* args)
{
void on_reply(
struct uxrSession * session,
uxrObjectId object_id,
uint16_t request_id,
uint16_t reply_id,
struct ucdrBuffer * ub,
uint16_t length,
void * args)
{
(void)session;
(void)object_id;
(void)args;

struct rmw_uxrce_mempool_item_t * client_item = client_memory.allocateditems;
while (client_item != NULL) {
rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)client_item->data;
if (custom_client->request_id == request_id)
{
if (custom_client->request_id == request_id) {
custom_client->micro_buffer_lenght[custom_client->history_write_index] = length;
ucdr_deserialize_array_uint8_t(ub, custom_client->micro_buffer[custom_client->history_write_index], length);
ucdr_deserialize_array_uint8_t(
ub,
custom_client->micro_buffer[custom_client->history_write_index], length);
custom_client->reply_id[custom_client->history_write_index] = reply_id;

// TODO (Pablo): Circular overlapping buffer implemented: use qos
if (custom_client->micro_buffer_in_use && custom_client->history_write_index == custom_client->history_read_index){
custom_client->history_read_index = (custom_client->history_read_index + 1) % RMW_UXRCE_MAX_HISTORY;
// TODO(pablogs9): Circular overlapping buffer implemented: use qos
if (custom_client->micro_buffer_in_use &&
custom_client->history_write_index == custom_client->history_read_index)
{
custom_client->history_read_index = (custom_client->history_read_index + 1) %
RMW_UXRCE_MAX_HISTORY;
}

custom_client->history_write_index = (custom_client->history_write_index + 1) % RMW_UXRCE_MAX_HISTORY;
custom_client->history_write_index = (custom_client->history_write_index + 1) %
RMW_UXRCE_MAX_HISTORY;
custom_client->micro_buffer_in_use = true;

break;
}
client_item = client_item->next;
}
client_item = client_item->next;
}
}
}
Loading