Skip to content
This repository was archived by the owner on Sep 15, 2025. It is now read-only.

Added ping_pong example #23

Merged
jamoralp merged 9 commits into from
Jul 21, 2020
Merged

Added ping_pong example #23

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
76a0fae
Added ping_pong example
pablogs9 Apr 27, 2020
c053452
Update
pablogs9 Apr 28, 2020
34da55a
Migrated ping_pong to RCLC
pablogs9 Jun 16, 2020
754c363
Update apps
pablogs9 Jul 20, 2020
579dfbe
Fix colcon metas
pablogs9 Jul 20, 2020
b53ea12
FIx kconfig
pablogs9 Jul 21, 2020
839b284
Updates
pablogs9 Jul 21, 2020
0367155
Working pingpong
pablogs9 Jul 21, 2020
e7b2d41
Migrate to RCLC
pablogs9 Jul 21, 2020
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
1 change: 0 additions & 1 deletion examples/kobuki/kobuki_main.cxx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,6 @@
// for time ??? #include "builtin_interfaces/msg/time__struct.h"

// strings in imu message : frame_id
#include "rosidl_generator_c/string.h"

#include "kobuki_robot.h"

Expand Down
2 changes: 0 additions & 2 deletions examples/kobuki/kobuki_node.cxx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,6 @@

#include "uros/ros_util.h"
#include "kobuki_node.h"
#include <rosidl_generator_c/string_functions.h>
#include <rosidl_generator_c/primitives_sequence_functions.h>

static const float NOMINAL_BATTERY_VOLTAGE = 16.7f;

Expand Down
6 changes: 5 additions & 1 deletion examples/publisher/publisher_main.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,18 +9,22 @@ int main(int argc, char *argv[])
#else
int publisher_main(int argc, char* argv[])
#endif
{
{
rcl_ret_t rv;

rcl_init_options_t options = rcl_get_zero_initialized_init_options();
rv = rcl_init_options_init(&options, rcl_get_default_allocator());
printf("rcl_init_options_init\n");

if (RCL_RET_OK != rv) {
printf("rcl init options error: %s\n", rcl_get_error_string().str);
return 1;
}

rcl_context_t context = rcl_get_zero_initialized_context();
rv = rcl_init(argc, argv, &options, &context);
printf("rcl_init\n");

if (RCL_RET_OK != rv) {
printf("rcl initialization error: %s\n", rcl_get_error_string().str);
return 1;
Expand Down
18 changes: 18 additions & 0 deletions examples/uros_pingpong/Kconfig
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
config UROS_PINGPONG_EXAMPLE
bool "micro-ROS Ping Pong"
default n
depends on UROS
---help---
micro-ROS Ping Pong sample app

if UROS_PINGPONG_EXAMPLE

config UROS_PINGPONG_EXAMPLE_PROGNAME
string "Program name"
default "uros_ping_pong"
depends on BUILD_KERNEL
---help---
This is the name of the program that will be use when the NSH ELF
program is installed.

endif
3 changes: 3 additions & 0 deletions examples/uros_pingpong/Make.defs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
ifeq ($(CONFIG_UROS_PINGPONG_EXAMPLE),y)
CONFIGURED_APPS += examples/uros_pingpong
endif
25 changes: 25 additions & 0 deletions examples/uros_pingpong/Makefile
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
-include $(TOPDIR)/Make.defs

#Set the scheduler priority for the app.
CONFIG_UROS_PINGPONG_EXAMPLE_PRIORITY ?= SCHED_PRIORITY_DEFAULT
#Set the stack size to the app. The minimum stack size on NuttX for a micro-ROS App is 65000 bytes
CONFIG_UROS_PINGPONG_EXAMPLE_STACKSIZE ?= 65000

#This is the name of the app on the NSH console
APPNAME = uros_pingpong
PRIORITY = $(CONFIG_UROS_PINGPONG_EXAMPLE_PRIORITY)
STACKSIZE = $(CONFIG_UROS_PINGPONG_EXAMPLE_STACKSIZE)

#Add the source files.
ASRCS =
CSRCS =
MAINSRC = app.c

CONFIG_UROS_PINGPONG_EXAMPLE_PROGNAME ?= uros_pingpong$(EXEEXT)
PROGNAME = $(CONFIG_UROS_PINGPONG_EXAMPLE_PROGNAME)
UROS_PINGPONG_INCLUDES = $(shell find $(APPDIR)/$(CONFIG_UROS_DIR)/install -type d -name include)
CFLAGS += ${shell $(INCDIR) $(INCDIROPT) "$(CC)" "$(UROS_PINGPONG_INCLUDES)"} -std=c99

MODULE = CONFIG_UROS_PINGPONG_EXAMPLE

include $(APPDIR)/Application.mk
143 changes: 143 additions & 0 deletions examples/uros_pingpong/app.c
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,143 @@
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <std_msgs/msg/header.h>

#include <stdio.h>
#include <unistd.h>
#include <time.h>

#define STRING_BUFFER_LEN 100

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}

rcl_publisher_t ping_publisher;
rcl_publisher_t pong_publisher;
rcl_subscription_t ping_subscriber;
rcl_subscription_t pong_subscriber;

std_msgs__msg__Header incoming_ping;
std_msgs__msg__Header outcoming_ping;
std_msgs__msg__Header incoming_pong;

int device_id;
int seq_no;
int pong_count;

void ping_timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
(void) last_call_time;

if (timer != NULL) {

seq_no = rand();
sprintf(outcoming_ping.frame_id.data, "%d_%d", seq_no, device_id);
outcoming_ping.frame_id.size = strlen(outcoming_ping.frame_id.data);

// Fill the message timestamp
struct timespec ts;
clock_gettime(CLOCK_REALTIME, &ts);
outcoming_ping.stamp.sec = ts.tv_sec;
outcoming_ping.stamp.nanosec = ts.tv_nsec;

// Reset the pong count and publish the ping message
pong_count = 0;
rcl_publish(&ping_publisher, (const void*)&outcoming_ping, NULL);
printf("Ping send seq %s\n", outcoming_ping.frame_id.data);
}
}

void ping_subscription_callback(const void * msgin)
{
const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin;

// Dont pong my own pings
if(strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) != 0){
printf("Ping received with seq %s. Answering.\n", msg->frame_id.data);
rcl_publish(&pong_publisher, (const void*)msg, NULL);
}
}


void pong_subscription_callback(const void * msgin)
{
const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin;

if(strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) == 0) {
pong_count++;
printf("Pong for seq %s (%d)\n", msg->frame_id.data, pong_count);
}
}


#if defined(BUILD_MODULE)
int main(int argc, char *argv[])
#else
int uros_pingpong_main(int argc, char* argv[])
#endif
{
rcl_allocator_t allocator = rcl_get_default_allocator();
rclc_support_t support;

// create init_options
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

// create node
rcl_node_t node = rcl_get_zero_initialized_node();
RCCHECK(rclc_node_init_default(&node, "pingpong_node", "", &support));

// Create a reliable ping publisher
RCCHECK(rclc_publisher_init_default(&ping_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping"));

// Create a best effort pong publisher
RCCHECK(rclc_publisher_init_best_effort(&pong_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong"));

// Create a best effort ping subscriber
RCCHECK(rclc_subscription_init_best_effort(&ping_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping"));

// Create a best effort pong subscriber
RCCHECK(rclc_subscription_init_best_effort(&pong_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong"));


// Create a 3 seconds ping timer timer,
rcl_timer_t timer = rcl_get_zero_initialized_timer();
RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(2000), ping_timer_callback));


// Create executor
rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator));

unsigned int rcl_wait_timeout = 1000; // in ms
RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));
RCCHECK(rclc_executor_add_timer(&executor, &timer));
RCCHECK(rclc_executor_add_subscription(&executor, &ping_subscriber, &incoming_ping, &ping_subscription_callback, ON_NEW_DATA));
RCCHECK(rclc_executor_add_subscription(&executor, &pong_subscriber, &incoming_pong, &pong_subscription_callback, ON_NEW_DATA));

// Create and allocate the pingpong messages

char outcoming_ping_buffer[STRING_BUFFER_LEN];
outcoming_ping.frame_id.data = outcoming_ping_buffer;
outcoming_ping.frame_id.capacity = STRING_BUFFER_LEN;

char incoming_ping_buffer[STRING_BUFFER_LEN];
incoming_ping.frame_id.data = incoming_ping_buffer;
incoming_ping.frame_id.capacity = STRING_BUFFER_LEN;

char incoming_pong_buffer[STRING_BUFFER_LEN];
incoming_pong.frame_id.data = incoming_pong_buffer;
incoming_pong.frame_id.capacity = STRING_BUFFER_LEN;

device_id = rand();

rclc_executor_spin(&executor);

RCCHECK(rcl_publisher_fini(&ping_publisher, &node));
RCCHECK(rcl_publisher_fini(&pong_publisher, &node));
RCCHECK(rcl_subscription_fini(&ping_subscriber, &node));
RCCHECK(rcl_subscription_fini(&pong_subscriber, &node));
RCCHECK(rcl_node_fini(&node));
}
1 change: 0 additions & 1 deletion examples/uros_pong_server/pong_server.cxx
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
#include "pong_server.h"
#include "rosidl_generator_c/string_functions.h"
#include <sstream>

using namespace kobuki;
Expand Down
14 changes: 7 additions & 7 deletions uros/Kconfig
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ endif
if UROS_TRANSPORT_SERIAL
config UROS_SERIAL_PORT
string "Serial port to use"
default "/dev/ttyS1"
default "/dev/ttyS0"

endif

Expand All @@ -58,21 +58,21 @@ config UROS_MAX_NODES
default 2

config UROS_MAX_PUBLISHERS
int "Maximum number of publishers per node"
int "Maximum number of publishers"
default 2

config UROS_MAX_SUBSCRIPTIONS
int "Maximum number of subscriptions per node"
default 1
int "Maximum number of subscriptions"
default 2

config UROS_MAX_SERVICES
int "Maximum number of service handlers per node"
int "Maximum number of service handlers"
default 1
---help---
This is the maximum number of services per node. Please note that parameter service, if used, needs one of those.
This is the maximum number of services. Please note that parameter service, if used, needs one of those.

config UROS_MAX_CLIENTS
int "Maximum number of service clients per node"
int "Maximum number of service clients"
default 1

config UROS_MAX_HISTORY
Expand Down
3 changes: 1 addition & 2 deletions uros/Makefile
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,7 @@ colcon_compile: arm_toolchain.cmake rmw_config.meta
$(Q) cd $(UROS_DIR); \
colcon build \
--packages-ignore-regex=.*_cpp \
--metas colcon.meta \
--metas $(APPDIR)/uros/rmw_config.meta \
--metas $(UROS_DIR)/colcon.meta $(APPDIR)/uros/rmw_config.meta \
--cmake-args \
-DBUILD_SHARED_LIBS=OFF \
-DCMAKE_POSITION_INDEPENDENT_CODE=ON \
Expand Down