Action server example #1129
Comments
|
Feel free to contribute with a PR |
|
Hi @pablogs9, I have created an example (see at the end) and running it on Arduino Nicla Vision which have same processor as Arduino Portenta. I am not using Arduino Nano RP2040 connect for action server since the prebuild using colcon_verylowmem.meta and RMW_UXRCE_MAX_SERVICES is set to 0. I guess the Arduino Nicla Vision uses the colcon.meta as I can confirm that micro-ros_addtwoints_service example ran successfully and I was able to interact with the service. I am not sure if After running the action service example code, micro-ros-agent prints the log message (see below). But I am not able to see the action name in the Example Action Server Sketch: #include <mbed.h>
#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <example_interfaces/action/fibonacci.h>
using namespace mbed;
using namespace rtos;
#define LED_PIN 13
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
const char * goalResult[7] = { "", "", "", "", "succeeded", "canceled", "aborted"};
rclc_executor_t executor;
Thread th;
//char buff[100];
void error_loop()
{
for (;;)
{
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
delay(100);
}
}
void fibonacci_worker(void * args)
{
(void) args;
rclc_action_goal_handle_t * goal_handle = (rclc_action_goal_handle_t *) args;
rcl_action_goal_state_t goal_state;
example_interfaces__action__Fibonacci_SendGoal_Request * req =
(example_interfaces__action__Fibonacci_SendGoal_Request *) goal_handle->ros_goal_request;
example_interfaces__action__Fibonacci_GetResult_Response response = {0};
example_interfaces__action__Fibonacci_FeedbackMessage feedback;
if (req->goal.order < 2) {
goal_state = GOAL_STATE_ABORTED;
} else {
feedback.feedback.sequence.capacity = req->goal.order;
feedback.feedback.sequence.size = 0;
feedback.feedback.sequence.data =
(int32_t *) malloc(feedback.feedback.sequence.capacity * sizeof(int32_t));
feedback.feedback.sequence.data[0] = 0;
feedback.feedback.sequence.data[1] = 1;
feedback.feedback.sequence.size = 2;
for (size_t i = 2; i < (size_t) req->goal.order && !goal_handle->goal_cancelled; i++) {
feedback.feedback.sequence.data[i] = feedback.feedback.sequence.data[i - 1] +
feedback.feedback.sequence.data[i - 2];
feedback.feedback.sequence.size++;
//Serial.println("Publishing feedback");
rclc_action_publish_feedback(goal_handle, &feedback);
delay(500);
}
if (!goal_handle->goal_cancelled) {
response.result.sequence.capacity = feedback.feedback.sequence.capacity;
response.result.sequence.size = feedback.feedback.sequence.size;
response.result.sequence.data = feedback.feedback.sequence.data;
goal_state = GOAL_STATE_SUCCEEDED;
} else {
goal_state = GOAL_STATE_CANCELED;
}
}
//sprintf(buff, "Goal %d %s, sending result array\n", req->goal.order, goalResult[goal_state]);
//Serial.print(buff);
rcl_ret_t rc;
do {
rc = rclc_action_send_result(goal_handle, goal_state, &response);
delay(1000);
} while (rc != RCL_RET_OK);
free(feedback.feedback.sequence.data);
}
rcl_ret_t handle_goal(rclc_action_goal_handle_t * goal_handle, void * context)
{
(void) context;
example_interfaces__action__Fibonacci_SendGoal_Request * req =
(example_interfaces__action__Fibonacci_SendGoal_Request *) goal_handle->ros_goal_request;
// Too big, rejecting
if (req->goal.order > 200) {
//sprintf(buff, "Goal %d rejected\n", req->goal.order);
//Serial.print(buff);
return RCL_RET_ACTION_GOAL_REJECTED;
}
//sprintf(buff, "Goal %d accepted\n", req->goal.order);
//Serial.print(buff);
// pthread_t * thread_id = malloc(sizeof(pthread_t));
// pthread_create(thread_id, NULL, fibonacci_worker, goal_handle);
th.start(callback(fibonacci_worker, goal_handle));
//th.start(fibonacci_worker);
return RCL_RET_ACTION_GOAL_ACCEPTED;
}
bool handle_cancel(rclc_action_goal_handle_t * goal_handle, void * context)
{
(void) context;
(void) goal_handle;
return true;
}
void setup()
{
set_microros_transports();
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);
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;
RCCHECK(rclc_node_init_default(&node, "fibonacci_action_server_rcl", "", &support));
// Create action service
rclc_action_server_t action_server;
RCCHECK(
rclc_action_server_init_default(
&action_server,
&node,
&support,
ROSIDL_GET_ACTION_TYPE_SUPPORT(example_interfaces, Fibonacci),
"fibonacci"
));
// Create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
example_interfaces__action__Fibonacci_SendGoal_Request ros_goal_request[10];
RCCHECK(
rclc_executor_add_action_server(
&executor,
&action_server,
10,
ros_goal_request,
sizeof(example_interfaces__action__Fibonacci_SendGoal_Request),
handle_goal,
handle_cancel,
(void *) &action_server));
}
void loop()
{
delay(100);
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}
|
|
Actions require 3 services server (goal, cancel and result service servers) and 2 publishers (feedback and status). Your micro-ROS Agent is only creating one service server, you should increase those values in the Could you try this? |
|
I have changed the colcon.meta to have 3 services as below and rebuilt the library. Now I am able to see the action name in the list so I think the action server has started and listed. But the command below says no servers. I tried to send goal but it failed. Is the command line correct? The micro-ros-agent log: |
This can be related to graph manager, but is should work in any case.
Everything seems to be correct, be carefull with the espaces: `
|
|
Thank @pablogs9 for the correct syntax. The terminal shows the result above forever. I guess it should exit after sending goal. How can I print request in the arduino sketch? I tried Serial.println but it hangs. microROS Agent log: |
|
try with And do not use Serial.println in your skect! It is being used as transport by micro-ROS and it will confuse the agent! |
|
With the And finally it goes inside this loop and never comes out. How can I check the req->goal.order value? |
|
I found one problem in your code, all the local variables that you are defining in function Please set all variables as global: rcl_allocator_t allocator;
rclc_support_t support;
rcl_node_t node;
rclc_action_server_t action_server;
example_interfaces__action__Fibonacci_SendGoal_Request ros_goal_request[10];
void setup()
{
set_microros_transports();
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);
allocator = rcl_get_default_allocator();
// create init_options
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
// create node
RCCHECK(rclc_node_init_default(&node, "fibonacci_action_server_rcl", "", &support));
// Create action service
RCCHECK(
rclc_action_server_init_default(
&action_server,
&node,
&support,
ROSIDL_GET_ACTION_TYPE_SUPPORT(example_interfaces, Fibonacci),
"fibonacci"
));
// Create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(
rclc_executor_add_action_server(
&executor,
&action_server,
10,
ros_goal_request,
sizeof(example_interfaces__action__Fibonacci_SendGoal_Request),
handle_goal,
handle_cancel,
(void *) &action_server));
}
void loop()
{
delay(100);
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
} |
|
Also, make sure that you are handling correctly the memory assigned to feedback and response messages. For testing purposes, you can set |
|
Thanks for the suggestion! I had moved all variable to the global scope except Now I can see the response without blocking. But it always works one time. The next send_goal request hangs again. Maybe I need to check the thread. |
|
Move |
|
Yes, I've moved before the last post. |
|
BTW, micro-ROS has not enabled the multithreading locks by default in |
|
Do we really need thread? I have copied the example from the https://github.com/micro-ROS/micro-ROS-demos/blob/humble/rclc/fibonacci_action_server/main.c which used thread so I have ported the code accordingly. |
|
It depends on your use case... Here I would try to lock each call to micro-ROS API in |
|
I tried to wrap each API call inside the
Also, inside the |
|
Try: Also, try to handle with static memory instead of |
|
Now I am using static memory of size 5 and only testing order = 5. Still not working. |
|
Share your new code with this modifications |
|
Here you go: #include <mbed.h>
#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <example_interfaces/action/fibonacci.h>
using namespace mbed;
using namespace rtos;
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
const char * goalResult[7] = { "", "", "", "", "succeeded", "canceled", "aborted"};
rclc_executor_t executor;
rcl_allocator_t allocator;
rclc_support_t support;
rcl_node_t node;
rclc_action_server_t action_server;
example_interfaces__action__Fibonacci_SendGoal_Request ros_goal_request[10];
int32_t data[5] = {0};
Thread th;
Mutex mtx;
void error_loop() {
for (;;) {
digitalWrite(LEDR, !digitalRead(LEDR));
delay(100);
}
}
void fibonacci_worker(void * args)
{
(void) args;
rclc_action_goal_handle_t * goal_handle = (rclc_action_goal_handle_t *) args;
rcl_action_goal_state_t goal_state;
example_interfaces__action__Fibonacci_SendGoal_Request * req =
(example_interfaces__action__Fibonacci_SendGoal_Request *) goal_handle->ros_goal_request;
example_interfaces__action__Fibonacci_GetResult_Response response = {0};
example_interfaces__action__Fibonacci_FeedbackMessage feedback;
if (req->goal.order < 2) {
goal_state = GOAL_STATE_ABORTED;
digitalWrite(LEDR, LOW);
} else {
feedback.feedback.sequence.capacity = req->goal.order;
feedback.feedback.sequence.size = 0;
// feedback.feedback.sequence.data =
// (int32_t *) malloc(feedback.feedback.sequence.capacity * sizeof(int32_t));
feedback.feedback.sequence.data = data;
feedback.feedback.sequence.data[0] = 0;
feedback.feedback.sequence.data[1] = 1;
feedback.feedback.sequence.size = 2;
for (size_t i = 2; i < (size_t) req->goal.order && !goal_handle->goal_cancelled; i++) {
feedback.feedback.sequence.data[i] = feedback.feedback.sequence.data[i - 1] +
feedback.feedback.sequence.data[i - 2];
feedback.feedback.sequence.size++;
mtx.lock();
rclc_action_publish_feedback(goal_handle, &feedback);
mtx.unlock();
delay(500);
}
if (!goal_handle->goal_cancelled) {
response.result.sequence.capacity = feedback.feedback.sequence.capacity;
response.result.sequence.size = feedback.feedback.sequence.size;
response.result.sequence.data = feedback.feedback.sequence.data;
goal_state = GOAL_STATE_SUCCEEDED;
digitalWrite(LEDG, LOW);
} else {
goal_state = GOAL_STATE_CANCELED;
digitalWrite(LEDB, LOW);
}
}
rcl_ret_t rc;
do {
mtx.lock();
rc = rclc_action_send_result(goal_handle, goal_state, &response);
mtx.unlock();
delay(1000);
} while (rc != RCL_RET_OK);
// if (goal_state != GOAL_STATE_ABORTED) {
// free(feedback.feedback.sequence.data);
// }
// th.terminate();
digitalWrite(LEDR, HIGH);
digitalWrite(LEDG, HIGH);
digitalWrite(LEDB, HIGH);
}
rcl_ret_t handle_goal(rclc_action_goal_handle_t * goal_handle, void * context)
{
(void) context;
example_interfaces__action__Fibonacci_SendGoal_Request * req =
(example_interfaces__action__Fibonacci_SendGoal_Request *) goal_handle->ros_goal_request;
// Too big, rejecting
if (req->goal.order > 200) {
return RCL_RET_ACTION_GOAL_REJECTED;
}
th.start(callback(fibonacci_worker, goal_handle));
return RCL_RET_ACTION_GOAL_ACCEPTED;
}
bool handle_cancel(rclc_action_goal_handle_t * goal_handle, void * context)
{
(void) context;
(void) goal_handle;
return true;
}
void setup()
{
set_microros_transports();
pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, HIGH);
pinMode(LEDR, OUTPUT);
digitalWrite(LEDR, HIGH);
pinMode(LEDG, OUTPUT);
digitalWrite(LEDG, HIGH);
pinMode(LEDB, OUTPUT);
digitalWrite(LEDB, HIGH);
delay(1000);
allocator = rcl_get_default_allocator();
// create init_options
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
// create node
RCCHECK(rclc_node_init_default(&node, "fibonacci_action_server_rcl", "", &support));
// Create action service
RCCHECK(
rclc_action_server_init_default(
&action_server,
&node,
&support,
ROSIDL_GET_ACTION_TYPE_SUPPORT(example_interfaces, Fibonacci),
"fibonacci"
));
// Create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(
rclc_executor_add_action_server(
&executor,
&action_server,
10,
ros_goal_request,
sizeof(example_interfaces__action__Fibonacci_SendGoal_Request),
handle_goal,
handle_cancel,
(void *) &action_server));
}
void loop()
{
delay(100);
mtx.lock();
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
mtx.unlock();
} |
|
What is the status? This works but only once right? |
|
Any This works only one time and hangs forever in the next calls.
|
|
I would recommend you to try to avoid the thread. Let's check if this solves the lock. |
|
Btw, you are calling start multiple times in the same thread object. Check documentation
|
|
I think every time a goal request comes a thread is started. Do you mean there should be only one thread running forever and accept argument using a queue? Or, the |
|
A Thread object can only be started once. Please read the RTOS documentation |
|
You are right. This was a Thread start issue. I was starting the same Thread object which worked once as expected but failed to start next time. I have moved the thread start call to the |
|
Ok, Im closing |
There is no action server example.
I would like to implement an action server to drive actuator using MoveIt2 JointTrajectory type request.
The text was updated successfully, but these errors were encountered: