rospy Odometry天坑小计

rospy Odometry天坑小计

在使用 python 自己搭建 ros 控制小车的上位机接口时，遇到了一个非常奇怪的问题，发布里程计的时候，一旦里程计得到订阅，发布者就会报 ndarray 属性找不到错误，乍一看着实让人脑瓜疼，明明处理的是四元数，为什么会和$numpy$扯上关系。百度和$CSDN$上并没有找到相关问题的记录，有人发出来的示例也都没有处理这个问题。这里记录一下，既是笔记，也是警示自己，处理这些情况的时候一定不能先入为主，还是得谨慎思考。

首先上图，报错截图如下，充满迷惑：

引发这个错误的源代码如下：

odom = nav_msgs.msg.Odometry()
odom.header.stamp = self.last_t
odom.header.frame_id = "odom"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0.
odom.pose.pose.orientation = odom_q
odom.child_frame_id = 'base_link'
odom.twist.twist.linear.x = self.vx
odom.twist.twist.linear.y = self.vy
odom.twist.twist.linear.z = 0.
odom.twist.twist.angular.z = self.w
print(odom)
self.odom_pub.publish(odom)

错误出现的第一时间翻阅了 Odometry 源码，并没有发现什么问题，CSDN 上也没有发现类似的情况。最终谷歌找到ROS问答社区发现问题，这里这个兄弟也遭遇了同样的情况。

问题出在上述代码第7行，其中 odom.pose.pose.orientation 是一个标准的四元数类型，而 odom_q 却是一个 numpy 的数组，因此在消息内部处理的时候遭遇该错误。

而这个 odom_q 是来自于下面这样一行变换，这句的作用是通过欧拉角得到四元数，但是这个函数有点反人类，它返回的四元数使用 ndarray 表示的，而不是标准的四元数，这于$C++$中的完全不一样，从$C++$转过来轻易不会注意到这个问题。

odom_q = tf.transformations.quaternion_from_euler(0, 0, self.thelta)

(也不知道作者和管理者是怎么想的，这也太坑了)

找到问题之后，问题就好办了，正确的写法如下：

odom.pose.pose.orientation.x = odom_q[0]
odom.pose.pose.orientation.y = odom_q[1]
odom.pose.pose.orientation.z = odom_q[2]
odom.pose.pose.orientation.w = odom_q[3]

至此，问题都得到解决。

参考文章：Error assigning a python quaternion - ROS Answers: Open Source Q&A Forum