接上文:
【轻量级开源ROS 的机器人设备(5)】--(1)拟议的框架——µROS节点
四、开发工具
为了方便用户应用程序的开发,一个代码生成器,一个 堆栈使用分析器和演示项目包含在框架中包裹。
4.1 代码生成器
手工编写编组过程和主题/服务处理例程可以 需要很多时间,而且这是一个容易出错的任务。编组的产生 过程可以通过处理传递的消息描述符来自动化 与 ROS 包。此外,处理程序例程共享一个通用框架, 可以被利用和重用。既然如此,生成工具,urogen (作为 Python 2.7 脚本 urosgen.py 实现)被开发出来。
4.2 编译流程
图 5.3 中描述的编译流程非常简单。该工具通过一个文件(清单 B.11 中的完整示例)进行配置,其中列出了主题/服务名称和类型,以及一些选项。一旦涉及的消息类型名称已知,该工具就会从已安装 ROS 包的相关 .msg 和 .srv 文件中加载它们的描述。最后,处理消息类型,从而产生编组和解组函数。为配置文件中列出的那些名称生成主题和服务处理程序存根。该工具还为 Doxygen 生成详细的自我文档(未在列表示例和大多数评论中报告)。
4.3 名称修改
所有 ROS 名称都具有类似路径的结构,以 /(斜杠)作为分隔符。 C标识符不能包含它,因此通过将 / 替换为来破坏 ROS 名称__(双下划线)。主题和服务错位名称总是以 __ 开头,因为它们有一个隐含/开头。
4.4 消息
通过调用 rosmsg show 从它的 .msg 文件加载消息类型,它删除注释并具有简洁的语法。其固有结构映射到 C 结构,其中损坏的名称以 msg__ (msg_name) 为前缀。原始类型被直接映射,而嵌套消息类型被声明分别地。为了符合C语言,结构体定义在拓扑顺序;这是可以做到的,因为 ROS 类型不能有循环依赖项。
struct msg__rosgraph_msgs__Log {
2 struct msg__std_msgs__Header header;
3 uint8_t level;
4 UrosString name;
5 UrosString msg;
6 UrosString file;
7 UrosString function;
8 uint32_t line;
9 UROS_VARARR(UrosString) topics;
10 };
11
12 #define msg__rosgraph_msgs__Log__DEBUG ((uint8_t)1)
13 #define msg__rosgraph_msgs__Log__INFO ((uint8_t)2)
14 #define msg__rosgraph_msgs__Log__WARN ((uint8_t)4)
15 #define msg__rosgraph_msgs__Log__ERROR ((uint8_t)8)
16 #define msg__rosgraph_msgs__Log__FATAL ((uint8_t)16)
Listing 5.1: Definition of the rosgraph_msgs/Log descriptor and its constant values
size_t length_msg__rosgraph_msgs__Log(
2 struct msg__rosgraph_msgs__Log *objp
3 ) {
4 size_t length = 0;
5 uint32_t i;
6
7 urosAssert(objp != NULL);
8
9 length += length_msg__std_msgs__Header(&objp->header);
10 length += sizeof(uint8_t);
11 length += sizeof(uint32_t) + objp->name.length;
12 length += sizeof(uint32_t) + objp->msg.length;
13 length += sizeof(uint32_t) + objp->file.length;
14 length += sizeof(uint32_t) + objp->function.length;
15 length += sizeof(uint32_t);
16 length += sizeof(uint32_t);
17 length += (size_t)objp->topics.length * sizeof(uint32_t);
18 for (i = 0; i < objp->topics.length; ++i) {
19 length += objp->topics.entriesp[i].length;
20 }
21
22 return length;
23 }
Listing 5.2: Stream length computation of a rosgraph_msgs/Log message
消息类型示例 rosgraph_msgs/Log 在清单 5.1 中。
代码生成器为以下操作创建函数:初始化,
清理、长度计算、编组和解组。
初始化函数(init_+msg_name,清单 5.3)设置初步的安全值。清理函数(clean_+msg_name,清单 5.4)
释放任何分配的字段,达到安全初始化状态。
长度计算函数(length_+msg_name,清单 5.2)计算
序列化消息的长度。
编组函数(send_+msg_name,清单 5.6)序列化消息内容并通过输出 TCPROS 流发送它们。反而,
解组函数(recv_+msg_name,清单 5.5)反序列化
从传入的 TCPROS 流中接收到消息
void init_msg__rosgraph_msgs__Log(
2 struct msg__rosgraph_msgs__Log *objp
3 ) {
4 uint32_t i;
5
6 urosAssert(objp != NULL);
7
8 init_msg__std_msgs__Header(&objp->header);
9 urosStringObjectInit(&objp->name);
10 urosStringObjectInit(&objp->msg);
11 urosStringObjectInit(&objp->file);
12 urosStringObjectInit(&objp->function);
13 urosTcpRosArrayObjectInit((UrosTcpRosArray *)&objp->topics);
14 for (i = 0; i < objp->topics.length; ++i) {
15 urosStringObjectInit(&objp->topics.entriesp[i]);
16 }
17 }
Listing 5.3: Initialization of a rosgraph_msgs/Log descriptor
1 void clean_msg__rosgraph_msgs__Log(
2 struct msg__rosgraph_msgs__Log *objp
3 ) {
4 uint32_t i;
5
6 if (objp == NULL) { return; }
7
8 clean_msg__std_msgs__Header(&objp->header);
9 urosStringClean(&objp->name);
10 urosStringClean(&objp->msg);
11 urosStringClean(&objp->file);
12 urosStringClean(&objp->function);
13 for (i = 0; i < objp->topics.length; ++i) {
14 urosStringClean(&objp->topics.entriesp[i]);
15 }
16 urosTcpRosArrayClean((UrosTcpRosArray *)&objp->topics);
17 }
Listing 5.4: Cleaning function of a rosgraph_msgs/Log descriptor
1 uros_err_t recv_msg__rosgraph_msgs__Log(
2 UrosTcpRosStatus *tcpstp,
3 struct msg__rosgraph_msgs__Log *objp
4 ) {
5 uint32_t i;
6
7 urosAssert(tcpstp != NULL);
8 urosAssert(urosConnIsValid(tcpstp->csp));
9 urosAssert(objp != NULL);
10 #define _CHKOK { if (tcpstp->err != UROS_OK) { goto _error; } }
11
12 recv_msg__std_msgs__Header(tcpstp, &objp->header); _CHKOK
13 urosTcpRosRecvRaw(tcpstp, objp->level); _CHKOK
14 urosTcpRosRecvString(tcpstp, &objp->name); _CHKOK
15 urosTcpRosRecvString(tcpstp, &objp->msg); _CHKOK
16 urosTcpRosRecvString(tcpstp, &objp->file); _CHKOK
17 urosTcpRosRecvString(tcpstp, &objp->function); _CHKOK
18 urosTcpRosRecvRaw(tcpstp, objp->line); _CHKOK
19 urosTcpRosArrayObjectInit((UrosTcpRosArray *)&objp->topics);
20 urosTcpRosRecvRaw(tcpstp, objp->topics.length); _CHKOK
21 objp->topics.entriesp = urosArrayNew(objp->topics.length,
22 UrosString);
23 if (objp->topics.entriesp == NULL) { tcpstp->err = UROS_ERR_NOMEM; goto _error; }
24 for (i = 0; i < objp->topics.length; ++i) {
25 urosTcpRosRecvString(tcpstp, &objp->topics.entriesp[i]); _CHKOK
26 }
27
28 return tcpstp->err = UROS_OK;
29 _error:
30 clean_msg__rosgraph_msgs__Log(objp);
31 return tcpstp->err;
32 #undef _CHKOK
33 }
Listing 5.5: Reception and unmarshaling of a rosgraph_msgs/Log message
1 uros_err_t send_msg__rosgraph_msgs__Log(
2 UrosTcpRosStatus *tcpstp,
3 struct msg__rosgraph_msgs__Log *objp
4 ) {
5 uint32_t i;
6
7 urosAssert(tcpstp != NULL);
8 urosAssert(urosConnIsValid(tcpstp->csp));
9 urosAssert(objp != NULL);
10 #define _CHKOK { if (tcpstp->err != UROS_OK) { return tcpstp->err; } }
11
12 send_msg__std_msgs__Header(tcpstp, &objp->header); _CHKOK
13 urosTcpRosSendRaw(tcpstp, objp->level); _CHKOK
14 urosTcpRosSendString(tcpstp, &objp->name); _CHKOK
15 urosTcpRosSendString(tcpstp, &objp->msg); _CHKOK
16 urosTcpRosSendString(tcpstp, &objp->file); _CHKOK
17 urosTcpRosSendString(tcpstp, &objp->function); _CHKOK
18 urosTcpRosSendRaw(tcpstp, objp->line); _CHKOK
19 urosTcpRosSendRaw(tcpstp, objp->topics.length); _CHKOK
20 for (i = 0; i < objp->topics.length; ++i) {
21 urosTcpRosSendString(tcpstp, &objp->topics.entriesp[i]); _CHKOK
22 }
23
24 return tcpstp->err = UROS_OK;
25 #undef _CHKOK
26 }
Listing 5.6: Marshaling and transmission of a rosgraph_msgs/Log message
4.5 服务讯息
服务消息是一对普通的输入/输出消息,因此被拆分为输入结构(in_srv__ + 损坏的名称,in_name)和输出结构(out_srv__ + mangled name, out_name),如清单 5.7 所示。他们是
通过调用 rossrv show 从相关的 .srv 文件加载,类似于普通消息。
每个子消息都有专门的初始化(清单 5.9)、清理(清单 5.10)和长度计算(清单 5.8)函数,以及上面看到的赋值和前缀。
只有输入消息有解组函数(recv_+in_name,清单 5.11),而只有输出消息具有编组功能(send_+out_name,清单 5.12)。
4.6 类型注册
urosMsgTypesRegStaticTypes() 过程将所有消息类型注册到它们各自的静态寄存器(参见第 5.2.1 节),如清单 5.13 所示。
4.7 处理例程
处理例程,除了服务调用的例程,应该在内部工作他们自己的线程。用户可以决定是否放置本地消息变量在堆栈或堆中,优化和协调内存管理;根据经验,小消息描述符放在堆栈上,大消息堆上的描述符。
4.8 主题发布者
主题发布者例程,pub_tpc + mangled name,生成并向订阅者发送纯消息,直到断开连接。生成的模板可以在清单 5.14 中看到,其中消息描述符被分配到堆中。用户只需要移除假人循环语句,并填写 msgp 描述符字段。
4.9 主题订阅者
另一方面,主题订阅者例程 sub_tpc+ 损坏的名称,不断接收和处理消息,直到断开连接。
1 struct in_srv__turtlesim__Spawn {
2 float x;
3 float y;
4 float theta;
5 UrosString name;
6 };
7
8 struct out_srv__turtlesim__Spawn {
9 UrosString name;
10 };
Listing 5.7: Definition of the turtlesim/Spawn descriptors
1 size_t length_in_srv__turtlesim__Spawn(
2 struct in_srv__turtlesim__Spawn *objp
3 ) {
4 size_t length = 0;
5
6 urosAssert(objp != NULL);
7
8 length += sizeof(float);
9 length += sizeof(float);
10 length += sizeof(float);
11 length += sizeof(uint32_t) + objp->name.length;
12
13 return length;
14 }
15
16 size_t length_out_srv__turtlesim__Spawn(
17 struct out_srv__turtlesim__Spawn *objp
18 ) {
19 size_t length = 0;
20
21 urosAssert(objp != NULL);
22
23 length += sizeof(uint32_t) + objp->name.length;
24
25 return length;
26 }
Listing 5.8: Stream length computation of turtlesim/Spawn messages
如清单 5.15 所示,用户只需要处理收到的消息,msgp。
Service publishers 服务发布者,pub_srv + mangled name,接收请求消息(in_name),处理,然后发送响应如果成功则为消息 (out_name),如果不成功则为错误字符串。如果尝试仍然存在,则重复这些操作直到断开连接。
清单 5.16 是服务发布者生成的模板。这是一个工会消息接收和传输,因此用户必须为两者提供代码请求处理和响应生成。
4.10 服务调用
服务调用例程 call_srv + 损坏的名称由客户端执行。它向发布者发送请求,然后接收响应或错误字符串。与在其主体内生成或处理消息的其他处理程序例程相反,服务调用例程仅用于通信;消息描述符也在外面分配。清单 5.17 中显示了生成的处理程序的示例。用户不需要做任何事情,但建议在收到响应之前释放请求描述符,以减少内存使用。
void init_in_srv__turtlesim__Spawn(
2 struct in_srv__turtlesim__Spawn *objp
3 ) {
4 urosAssert(objp != NULL);
5
6 urosStringObjectInit(&objp->name);
7 }
8
9 void init_out_srv__turtlesim__Spawn(
10 struct out_srv__turtlesim__Spawn *objp
11 ) {
12 urosAssert(objp != NULL);
13
14 urosStringObjectInit(&objp->name);
15 }
Listing 5.9: Initialization of turtlesim/Spawn descriptors
1 void clean_in_srv__turtlesim__Spawn(
2 struct in_srv__turtlesim__Spawn *objp
3 ) {
4 urosAssert(objp != NULL);
5
6 urosStringClean(&objp->name);
7 }
8
9 void clean_out_srv__turtlesim__Spawn(
10 struct out_srv__turtlesim__Spawn *objp
11 ) {
12 urosAssert(objp != NULL);
13
14 urosStringClean(&objp->name);
15 }
Listing 5.10: Cleaning function of turtlesim/Spawn descriptors
1 uros_err_t recv_in_srv__turtlesim__Spawn(
2 UrosTcpRosStatus *tcpstp,
3 struct in_srv__turtlesim__Spawn *objp
4 ) {
5 urosAssert(tcpstp != NULL);
6 urosAssert(urosConnIsValid(tcpstp->csp));
7 urosAssert(objp != NULL);
8 #define _CHKOK { if (tcpstp->err) { goto _error; } }
9
10 urosTcpRosRecvRaw(tcpstp, objp->x); _CHKOK
11 urosTcpRosRecvRaw(tcpstp, objp->y); _CHKOK
12 urosTcpRosRecvRaw(tcpstp, objp->theta); _CHKOK
13 urosTcpRosRecvString(tcpstp, &objp->name); _CHKOK
14
15 return tcpstp->err = UROS_OK;
16 _error:
17 clean_in_srv__turtlesim__Spawn(objp);
18 return tcpstp->err;
19 #undef _CHKOK
20 }
Listing 5.11: Reception and unmarshaling of a turtlesim/Spawn request message
1 uros_err_t send_out_srv__turtlesim__Spawn(
2 UrosTcpRosStatus *tcpstp,
3 struct out_srv__turtlesim__Spawn *objp
4 ) {
5 urosAssert(tcpstp != NULL);
6 urosAssert(urosConnIsValid(tcpstp->csp));
7 urosAssert(objp != NULL);
8 #define _CHKOK { if (tcpstp->err) { return tcpstp->err; } }
9
10 urosTcpRosSendString(tcpstp, &objp->name); _CHKOK
11
12 return tcpstp->err = UROS_OK;
13 #undef _CHKOK
14 }
Listing 5.12: Marshaling and transmission of a turtlesim/Spawn response message
1 void urosMsgTypesRegStaticTypes(void) {
2
3 urosRegisterStaticMsgTypeSZ("rosgraph_msgs/Log",
4 NULL, "acffd30cd6b6de30f120938c17c593fb");
5
6 urosRegisterStaticMsgTypeSZ("std_msgs/Header",
7 NULL, "2176decaecbce78abc3b96ef049fabed");
8
9 urosRegisterStaticSrvTypeSZ("turtlesim/Spawn",
10 NULL, "0b2d2e872a8e2887d5ed626f2bf2c561");
11 }
Listing 5.13: Registration of the static types used in the examples above
1 uros_err_t pub_tpc__rosout(UrosTcpRosStatus *tcpstp) {
2
3 UROS_TPC_INIT_H(msg__rosgraph_msgs__Log);
4
5 while (!urosTcpRosStatusCheckExit(tcpstp)) {
6 /* TODO: Generate the contents of the message.*/
7 urosThreadSleepSec(1); continue; /* TODO: Remove this dummy line.*/
8
9 UROS_MSG_SEND_LENGTH(msgp, msg__rosgraph_msgs__Log);
10 UROS_MSG_SEND_BODY(msgp, msg__rosgraph_msgs__Log);
11
12 clean_msg__rosgraph_msgs__Log(msgp);
13 }
14 tcpstp->err = UROS_OK;
15
16 _finally:
17 UROS_TPC_UNINIT_H(msg__rosgraph_msgs__Log);
18 return tcpstp->err;
19 }
Listing 5.14: Generated handler template for a common /rosout publisher
1 uros_err_t sub_tpc__rosout(UrosTcpRosStatus *tcpstp) {
2
3 UROS_TPC_INIT_H(msg__rosgraph_msgs__Log);
4
5 while (!urosTcpRosStatusCheckExit(tcpstp)) {
6 UROS_MSG_RECV_LENGTH();
7 UROS_MSG_RECV_BODY(msgp, msg__rosgraph_msgs__Log);
8
9 /* TODO: Process the received message.*/
10
11 clean_msg__rosgraph_msgs__Log(msgp);
12 }
13 tcpstp->err = UROS_OK;
14
15 _finally:
16 UROS_TPC_UNINIT_H(msg__rosgraph_msgs__Log);
17 return tcpstp->err;
18 }
Listing 5.15: Generated handler template for a /rosout subscriber
1 uros_err_t pub_srv__reconfigure(UrosTcpRosStatus *tcpstp) {
2
3 UROS_SRV_INIT_HISO(in_srv__dynamic_reconfigure__Reconfigure,
4 out_srv__dynamic_reconfigure__Reconfigure);
5
6 do {
7 UROS_MSG_RECV_LENGTH();
8 UROS_MSG_RECV_BODY(inmsgp, in_srv__dynamic_reconfigure__Reconfigure);
9
10 /* TODO: Process the request message.*/
11 tcpstp->err = UROS_OK;
12 urosStringClean(&tcpstp->errstr);
13 okByte = 1;
14
15 clean_in_srv__dynamic_reconfigure__Reconfigure(inmsgp);
16
17 /* TODO: Generate the contents of the response message.*/
18
19 UROS_SRV_SEND_OKBYTE_ERRSTR();
20 UROS_MSG_SEND_LENGTH(&outmsg, out_srv__dynamic_reconfigure__Reconfigure);
21 UROS_MSG_SEND_BODY(&outmsg, out_srv__dynamic_reconfigure__Reconfigure);
22
23 clean_out_srv__dynamic_reconfigure__Reconfigure(&outmsg);
24 } while (tcpstp->topicp->flags.persistent &&
25 !urosTcpRosStatusCheckExit(tcpstp));
26 tcpstp->err = UROS_OK;
27
28 _finally:
29 UROS_SRV_UNINIT_HISO(in_srv__dynamic_reconfigure__Reconfigure,
30 out_srv__dynamic_reconfigure__Reconfigure);
31 return tcpstp->err;
32 }
Listing 5.16: Generated handler template for a service publisher
1 uros_err_t call_srv__reconfigure(
2 UrosTcpRosStatus *tcpstp,
3 struct in_srv__dynamic_reconfigure__Reconfigure *inmsgp,
4 struct out_srv__dynamic_reconfigure__Reconfigure *outmsgp
5 ) {
6
7 UROS_SRVCALL_INIT(in_srv__dynamic_reconfigure__Reconfigure,
8 out_srv__dynamic_reconfigure__Reconfigure);
9
10 UROS_MSG_SEND_LENGTH(inmsgp, in_srv__dynamic_reconfigure__Reconfigure);
11 UROS_MSG_SEND_BODY(inmsgp, in_srv__dynamic_reconfigure__Reconfigure);
12
13 /* TODO: Dispose the contents of the request message.*/
14
15 UROS_SRV_RECV_OKBYTE();
16 UROS_MSG_RECV_LENGTH();
17 UROS_MSG_RECV_BODY(outmsgp, out_srv__dynamic_reconfigure__Reconfigure);
18
19 tcpstp->err = UROS_OK;
20 _finally:
21 return tcpstp->err;
22 }
Listing 5.17: Generated handler template for a client service call