在VSCode下利用PlateFormIO开发Arduino的MicroROS遇到的一些问题

简介

我是按照鱼香ROS的教程【3.搭建PlateFormIO开发环境】进行的，但是在进行的过程中，遇到了一些问题，这里记录下来，供有同样问题的同学进行参考。其实只要你使用的板子的MCU是ESP32，都可以按照他这个教程进行操作。

0.micro-ROS官方文档

遇到问题时，也可以查看一下官方的文档，可以学到一些函数的用法。
官方文档地址：https://micro.ros.org

1.左下角没有platformio的相关按钮

在第四节编译工程中，教程使用的vscode的左下角是有编译、上传的按钮的。但是我的没有。
教程的：
我的：

不过没关系，点击那个小蚂蚁，执行RROJECT TASKS–》General下面的命令，具有同样的效果。

后来又发现在右上角：

2.vscode没有串行监视器（Serial Monitor）

在【6.串口通信-接收实验】中，教程中是有这个串行监视器（Serial Monitor）的，但是我这边没有。

其实这个需要是个独立的拓展，需要自己独立安装。

3.关于trajectory_msgs/msg/joint_trajectory.hpp的相关问题

首先我们要明确，microros这个环境基本是基于c开发的，而不是cpp。所以，trajectory_msgs/msg/joint_trajectory.hpp这个cpp下的目录是没有的，取而代之的是trajectory_msgs/msg/joint_trajectory.h

4.关于control_msgs::action::FollowJointTrajectory的相关问题

假如我们想让我们的microros节点直接接入到moveit中，一种比较好的办法是直接在此节点上实现一个action_server,类型为control_msgs::action::FollowJointTrajectory。
但是目前鱼香ros的microros不支持该类型，而官方的也是不支持的（当使用官方的microros时，需要梯子或者fastgithub）。
不知道怎么回事，我看官方的【micro_ros_arduino】是有这个模块的。

我在上面发起了个提问，到时候再看看。【I couldn’t find the control_msg module in the compiled include folder】
实在没办法的话，可以暂时在电脑端创建个action_server,然后通过用service传给下位机吧。

4.1.方法一

20230612：他们今天回答了。

意思大概是假如这个库没有control_msgs这个模块，需要自己按照 https://github.com/micro-ROS/micro_ros_platformio#extra-packages 这里的说明来进行添加到编译步骤中。

ok，那我们自己操作一下。
呃，但是，在哪个路径下搞这个extra_packages.repos呢？😶
问了一下chatgpt

也就是在platformio.ini所在的目录下。
但是，extra_packages.repos应该如何写？
在 /home/yong/Desktop/arduino/hello_microros/.pio/libdeps/featheresp32/micro_ros_platformio/ci/extra_packages（你需要根据你自己的项目名字来决定这个路径，其实也就是你当前项目下的） 这个目录下是有一个extra_packages.repos。

我们用的是humble，他这个是galactic-devl与humble可能不匹配。我们去 https://github.com/ros-controls/control_msgs 看看
有humble的，那我们手动改一下：
ok，那就按照它的写法，自己在platformio.ini所在目录下，建立一个extra_packages文件夹，然后在里面新建一个extra_packages.repos文件，内容如下

repositories:
  control_msgs:
    type: git
    url: https://ghproxy.com/https://github.com/ros-controls/control_msgs
    version: humble

修改之后，手动将libmicroros文件夹删除，然后重新编译。
编译的时候，可能会发生类似这样的错误：

control_msgs clone failed: 
fatal: destination path '/home/yong/Desktop/arduino/hello_microros/.pio/libdeps/featheresp32/micro_ros_platformio/build/mcu/src/control_msgs' already exists and is not an empty directory.

这是因为在原来的 microros_utils/library_builder.py文件中，对extra_packages重复下载了（鱼香ros的：75行，139行；官方的：65行，123行 ）。需要删除其中一个才行。针对这个问题，我提交了一个push request：【 Update library_builder.py #105 】 😂
这个错误是鱼香ros这边才有，官方那边是没有的。原因在于第75行这个，与139行重复了。

4.2.方法二

另外分享一个我发现的一个办法吧（不太建议使用，相当于改人家的源代码，不好）：
直接修改 repositories.py

  +          Repository("control_msgs", "https://ghproxy.com/https://github.com/ros-controls/control_msgs", "humble"),

注意我目前使用的是鱼香ros的仓库，所以加上的网址是带【https://ghproxy.com/】的（起到加速、梯子的作用），假如你是用官方的，没必要加这个前缀（因为你都能够使用官方的了，应该就不存在墙的问题）。
修改之后，手动将libmicroros文件夹删除，

然后重新编译

顺利的话，等编译完成后，你就可以看到control_msgs了。

不是很懂为什么官方不直接加进去，难道是出于单片机资源紧张的考虑？
通过对比好像发现，control_msgs是属于第三方团队https://github.com/ros-controls的，并不是ros团队（https://github.com/ros2、https://github.com/micro-ROS）的亲儿子。所以默认没有包含进去？

5.关于moveit_msg的相关问题

moveit_msgs也是第三方的，默认是没有包含的。因此也需要自己加进去

  +          Repository("moveit_msgs", "https://ghproxy.com/https://github.com/ros-planning/moveit_msgs", "humble"),

直接加这个还是不得行的，因为它还需要其他的msg
因此，需要把extra_packages.repos搞成这样（关于extra_packages.repos，请查看本文4.1）：

repositories:
  control_msgs:
    type: git
    url: https://ghproxy.com/https://github.com/ros-controls/control_msgs
    version: humble
  object_recognition_msgs:
    type: git
    url: https://ghproxy.com/https://github.com/wg-perception/object_recognition_msgs
    version: ros2
  octomap_msgs:
    type: git
    url: https://ghproxy.com/https://github.com/OctoMap/octomap_msgs
    version: ros2
  moveit_msgs:
    type: git
    url: https://ghproxy.com/https://github.com/ros-planning/moveit_msgs
    version: humble

需要注意的是，上面的【version】 是填ros2还是填humble要根据人家仓库的分支命名来决定，不能随便填。
然后就有了：

6.publisher以及service不能同时实现的问题

按照教程，假如你想把发布话题、实现服务这两个功能一起放到同一个工程中：

...
  // 执行器添加服务
  rclc_executor_add_service(&executor, &service, &req, &res, service_callback);
...
  // 给执行器添加定时器
  rclc_executor_add_timer(&executor, &timer);
...

会发现只能实现其中一个功能，另外一个会被忽略。
这个其实只要改一下执行器的初始化语句就行：

  // 创建执行器
-  rclc_executor_init(&executor, &support.context, 1, &allocator);
+  rclc_executor_init(&executor, &support.context, 2, &allocator);

也就是把number_of_handles从1改成2.这个参数是控制执行器允许处理的对象数量，可以自己看一下源码。这也是对寸土寸金的单片机的妥协，也没办法，要控制好每一份资源。

7.创建action失败的原因

假如我们直接利用类似以下的代码创建action的话，大概率会返回RCL_RET_ERROR：

...
auto ret = rclc_action_server_init_default(
      &mActionServer, node, support,
      ROSIDL_GET_ACTION_TYPE_SUPPORT(control_msgs, FollowJointTrajectory),
      // "/my_group_controller/follow_joint_trajectory" // 不支持有效字符长度超过29的名字
      // "my/follow_joint_trajectory" // 双层也是支持的
      "follow_joint_trajectory"
      );

  if (ret != RCL_RET_OK) {
    return 10000 + ret;
    // return -1;
  }

  // microros没有AcceptedCallback ？
  ret = rclc_executor_add_action_server(executor, &mActionServer, 1, &mGoalReq, sizeof(mGoalReq),
                                        action_server_handle_goal,
                                        action_server_handle_cancel, nullptr);
  if (ret != RCL_RET_OK) {
    return 20000 + ret;
    // return -2;
  }
 ...

倒不是我们的代码有问题，而是，创建一个action，需要创建好几个service，而能创建的service数量，在编译时microros时已经被限制了，具体请查看：【 Unable to create two servers in micro_ros_platformio #103 】
解决办法是：
也就是修改colcon.meta、colcon_lowmem.meta、colcon_verylowmem.meta（因为我不知道它用哪个，所以我全改了） ,改成你觉得合适的数据。然后重新编译。
不过 假如看一下 extra_script.py 这个文件，里面有各个文件的对应。也就是我们改colcon.meta这个文件就行了。
同时也要注意第6点提到的number_of_handles问题。
处理好之后，你的action自然出现了。

8.关于action_name的字符长度被限制在29的问题

8.1 原因分析

本文第7节中的代码提到，action_name不支持有效字符长度超过29的名字。我是怎么知道的？我是一个个字符增加测试出来的😣。
那为啥是29？
因为创建action时会创建三个service，其中一个名叫xxx/_action/cancel_goal。
但是，查看创建service的init函数，假如你一层一层地查看源代码，会在一个叫 rmw_service.c 的文件中的第60行看到这玩意。
总之就是这句话限制了service_name的长度。
然后再继续深入，知道了这玩意：RMW_UXRCE_TOPIC_NAME_MAX_LENGTH
这玩意的值为60.
那 19 + 20(/_action/cancel_goal) + 1 = 50，那也没超过啊。。。
哦，漏了一段

继续追查到 expand_topic_name.c ，可以看到，service的最终名字是会混合命名空间、节点名称之类的。我之前的节点名称为【hello_microros】14个字节…好像又超太多了。唉，算了，总之，最终的service_name会比你原来的名字长一段，而且和节点名称、命名空间有关。
好累。

8.2.解决办法

通过查找，在 【/home/yong/Desktop/arduino/hello_microros/.pio/libdeps/featheresp32/micro_ros_platformio/build/mcu/src/rmw-microxrcedds/rmw_microxrcedds_c/CMakeLists.txt】 这个文件中出现了可以配置这个宏定义的地方。直接在这里修改肯定不行，因为这个是在编译时才下载下来的。
办法是回到metas/colcon.meta,修改成这样（修改的数字我暂时只是随意修改的，可能会导致一些未知的错误。但是在新的错误未出现之前，先用着）（已经出现问题了，详细请看10）：

{
    "names": {
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=10",
                "-DRMW_UXRCE_MAX_PUBLISHERS=10",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=10",
                "-DRMW_UXRCE_MAX_SERVICES=10",
                "-DRMW_UXRCE_MAX_CLIENTS=10",
                "-DRMW_UXRCE_MAX_HISTORY=10",
                "-DRMW_UXRCE_TRANSPORT=custom",

                "-DRMW_UXRCE_NODE_NAME_MAX_LENGTH=128",
                "-DRMW_UXRCE_TOPIC_NAME_MAX_LENGTH=128",
                "-DRMW_UXRCE_TYPE_NAME_MAX_LENGTH=128",
            ]
        },
        "microxrcedds_client":{
                "cmake-args": [
                    "-DUCLIENT_CUSTOM_TRANSPORT_MTU=1024",
                ]
        }
    }
}

慎重设置上面的 RMW_UXRCE_MAX_NODES，保持1就好了。否则非常耗资源。
编译，运行，长名字的action也就出现了。完美。
后来发现，别人已经发现过这个问题：【 Long action name does not work for action server #1138 】

这里我之所以执着于这个长名字，是因为我的moveit上位机指定的action名字为my_group_controller

9.关于代码调试、信息打印

相信很多人都希望micro_ros这边能够类似电脑端直接调用 RCLCPP_INFO() 之类的函数来打印信息、或者获取什么last_error之类来显示函数错误的原因（这一点在我查找实现action初始化失败的原因时深有体会😣）。
但是，遗憾的是，貌似暂时还没有很好的解决办法，哪怕你修改common.meta里面的
“-DRCUTILS_AVOID_DYNAMIC_ALLOCATION=OFF”，也是不行的（agent直接无法初始化micro_ros，然后单片机不断重启）；
目前只能解决printf的问题，办法肯定很多人想到了，就是创建一个string的话题发布者，然后在需要的地方发布信息就行：

'''
// 声明明话题发布者
rcl_publisher_t publisher;
// 声明消息结构体
std_msgs__msg__String pub_msg;
// 缓冲器
char strBuffer[256] = {};
...
  // 发布者初始化
  rclc_publisher_init_default(
      &publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
      "microros_log");
...
  // 填充自己的消息
  sprintf(strBuffer, "robot init ret = %d\r\n", ret);
  pub_msg.data.data = strBuffer;
  pub_msg.data.size = strlen(strBuffer);
  pub_msg.data.capacity = pub_msg.data.size + 1;

...
  // 派发消息
  rcl_ret_t ret = rcl_publish(&publisher, &pub_msg, NULL);

然后，你可以在上面的基础上写出下面这个很骚的用法：

// 用一个函数,直接sprinf+发布
int my_ros_log(char *formattedStr, ...)
{
  va_list args;
  
  // 初始化可变参数列表
  va_start(args, formattedStr);
  
  // 将格式字符串和可变参数列表转换为字符串
  vsnprintf(strBuffer, 256, formattedStr, args);

  // 结束可变参数列表的使用
  va_end(args);

  // // 这样是自己初始化的方式，下面的官方提供的方式也行，不过会复制多一份内存，感觉有点浪费
  pub_msg.data.data = strBuffer;
  pub_msg.data.size = strlen(strBuffer);
  pub_msg.data.capacity = pub_msg.data.size + 1;

  // // 需要 #include <micro_ros_utilities/string_utilities.h>
  // pub_msg.data = micro_ros_string_utilities_init(strBuffer);

  // 派发消息
  rcl_ret_t ret = rcl_publish(&publisher, &pub_msg, NULL);

  return ret;
}

这时候，你就可以在需要的地方直接使用了：

  my_ros_log("the traj points count:%d", points.size);

效果还是不错的：

10.关于action_server无法接收goal的问题

ESP32的资源：

HARDWARE: ESP32 240MHz, 320KB RAM, 4MB Flash

10.1.现象

如前面所述，我创建了一个control_msgs/action/FollowJointTrajectory类型的action： follow_joint_trajectory ，然后尝试通过下面的命令从电脑发送指令给esp32
这里稍微提一下，moveit发送和接受的角度数据都是以弧度表示的，要记住这一点。

ros2 action send_goal /my_group_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{
  trajectory: {
    joint_names: [joint1, joint2, joint3, joint4, joint5],
    points: [
      { positions: [0.1, 0.1, 0.1, 0.1, 0.1], time_from_start: { sec: 0, nanosec: 500 } },
      { positions: [0.2, 0.5, 0.2 ,0.2, 0.2], time_from_start: { sec: 5, nanosec: 500 } },
      { positions: [0.3, 0.3, 0.7, -0.5, 0.3], time_from_start: { sec: 7, nanosec: 500 } },
      { positions: [0.4, 0.4, 0.9, 0.4, 0.4], time_from_start: { sec: 8, nanosec: 500 } }
    ]
  }
}"

成功了，但是并没有完全成功。貌似已经发送了出去，但是没有收到返回，而且下位机的回调函数也没有被调用。

10.2.查找资料及分析

看了好几遍代码（参考【 Action server example #1129 】），觉得我的代码应该没啥问题，那到底是怎么回事？
再查找一下资料，发现有人讨论过类似的问题：【 Handling large action request gets failed #1145 】
里面提到RMW_UXRCE_STREAM_HISTORY、timer、超时的问题。
RMW_UXRCE_STREAM_HISTORY，这个参数貌似会影响数据通讯的缓存空间？
不能直接用32，会报错
region `dram0_0_seg’ overflowed by 316440 bytes
经过一番测试(优化代码、把不必要的东西去掉等等)，最后可以只能用8了，这个可能和其他参数设置有关系，最好自己动手试试（但是要注意是要2的n次方，比如4、8、16、32、64等等）。
此外，还要控制好超时的问题。

10.3.解决方案

总的来说，是因为整个action goal的数据量比较大，一方面留给数据传输缓存的空间要大一点，另外ros的超时时间也要留长一点。也就是空间、时间的问题。
时间方面：减少或者取消loop函数里面的delay、增加rclc_executor_spin_some的超时时间、其它。
其实我们对比一下【 Action server example #1129 】、【 Handling large action request gets failed #1145 】这两篇讨论的代码，就会发现，前一篇对ros_goal_request声明之后，就可以直接拿来用了，而后面一篇还需要用一个函数来申请ros_goal_request的空间：

而恰恰就是我没有调用这个函数来初始化，才导致出现了我上面所述的问题。
加上这个函数，就ok了。

另外，这是我目前的colcon.meta:

{
    "names": {
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=10",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=5",
                "-DRMW_UXRCE_MAX_SERVICES=6",
                "-DRMW_UXRCE_MAX_CLIENTS=1",
                "-DRMW_UXRCE_MAX_HISTORY=4",
                "-DRMW_UXRCE_TRANSPORT=custom",

                "-DRMW_UXRCE_TOPIC_NAME_MAX_LENGTH=100",
                "-DRMW_UXRCE_STREAM_HISTORY=8",
            ]
        },
        "microxrcedds_client":{
                "cmake-args": [
                    "-DUCLIENT_CUSTOM_TRANSPORT_MTU=1024",
                ]
        }
    }
}

为啥有时候需要申请空间，有时候不用呢？
（20231006，关于这个问题，我尝试解释了一下【关于micro-ROS中对消息的内存管理】）

11.关于action_server接收了goal，却无法Goal finished with status: xxx的问题

在按照本文第10小节，操作后，应该是能够发送了goal给下位机，而且下位机的【rclc_action_goal_handle_t】函数也被成功调用了。
但是，假如你在【rclc_executor_add_action_server】时，设置【handles_number】为n的话（并且【ros_goal_request】也正确地申请了空间），那么，反复发送了n次后，就无法再发送了，发送程序就一直卡在那里，不会出现 【Goal accepted with ID: 72718b32773f4a0583cb543b8f33d113】之类的信息。
这是因为没有调用【rclc_action_send_result】来告知系统该goal已经被处理，所以这个goal就一直占用着内存。等你申请的空间被占用完了，那自然无法再继续接收（受）goal了。
而且这个函数还不是一次调用就会成功，还需要反复调用，直到确认调用成功才行。
另外，这个函数也不应该在【rclc_action_goal_handle_t】就调用。在【rclc_action_goal_handle_t】里应该是判断是否接受该goal，接受的话，先记录好，然后返回接受值【RCL_RET_ACTION_GOAL_ACCEPTED】（不接受的话，直接返回【RCL_RET_ACTION_GOAL_REJECTED】，后面吊事都不用处理）；接着再在另外的线程、函数中执行电机的操作，然后再返回结果。
处理好的话，应该是可以得到类似的结果：

RCL_RET_ACTION_GOAL_REJECTED

RCL_RET_ACTION_GOAL_ACCEPTED

12.Controller is taking too long to execute trajectory (the expected upper 。。。问题

对于本人的项目来说，这个主要是因为moveit给我规划的路径的执行时间太短了，我的机械臂跑不了这么快。
针对这个问题，只需要修改config里面的joint_limits.yaml文件就行。具体数值要看实际需求。

总结

在单片机下面操作，一来要先申请空间，再操作；二来真的是1bit、1byte都要考虑清楚。

其他暂时没问题，遇到再补充。