目录
1，ROS坐标系、单位的相关概念
首先要补充下 ROS 里面的基本概念。ROS 使用的是 C++编写的，但是因为特殊需要使用golang 进行编程，然后连接到 ROS的服务上。
所以要知道 ROS 里面的消息的结构体，就可以正确地进行解析了。
ROS里有统一定义了一系列机器人系统常用的坐标系和单位。
Point(点)
# This contains the position of a point in free spacefloat64 xfloat64 yfloat64 z
Quaternion(四元数)
# This represents an orientation in free space in quaternion form.float64 xfloat64 yfloat64 zfloat64 w
Pose(姿态)
# A representation of pose in free space, composed of position and orientation. Point positionQuaternion orientation
Twist
# This expresses velocity in free space broken into its linear and angular parts.Vector3 linearVector3 angular
标识线速度和角速度。角速度的单位一般是rad（弧度），转角度 = rad*180/PI。
Odometry(里程计)
# This represents an estimate of a position and velocity in free space.# The pose in this message should be specified in the coordinate frame given by header.frame_id.# The twist in this message should be specified in the coordinate frame given by the child_frame_idHeader headerstring child_frame_idgeometry_msgs/PoseWithCovariance posegeometry_msgs/TwistWithCovariance twist
里程计包两个信息，一方面是位置和姿态，一个是直线速度和角速度，同时配有frame-id和timestamp。
frame-id是用来指示坐标系的？
base_link坐标系一般为表示机器人中心，为相对机器人的本体的坐标系，比如说雷达识别到前方xx米有障碍物，这个前方xx米就是相对机器人而言。
odom 坐标系odom坐标系标识机器人相对运动原点的位置，一般来说都是连续的。
2，接入ROS 的topic 后一定要找好对应的消息类，才可以成功解析
一定要找好相对应的消息类型，然后才可以正确地解析，否则程序报错。如果是普通消息使用的类型是：
标准的消息，包括一个 Data 字段。
https://pkg.go.dev/github.com/aler9/goroslib/pkg/msgs/std_msgs#String
//autogenerated:yes//nolint:revive,lllpackage std_msgsimport (“github.com/aler9/goroslib/pkg/msg”)type String struct {msg.Package `ros:”std_msgs”`Datastring}
再比如，里程 类：https://pkg.go.dev/github.com/aler9/goroslib/pkg/msgs/nav_msgs#Odometry
//autogenerated:yes//nolint:revive,lllpackage nav_msgsimport (“github.com/aler9/goroslib/pkg/msg””github.com/aler9/goroslib/pkg/msgs/geometry_msgs””github.com/aler9/goroslib/pkg/msgs/std_msgs”)type Odometry struct {msg.Package`ros:”nav_msgs”`Header std_msgs.HeaderChildFrameId stringPose geometry_msgs.PoseWithCovarianceTwistgeometry_msgs.TwistWithCovariance}
然后就可以按照这个类型的数据进行解析了。
3，找到里程信息之后需要，把四元数到欧拉角
参考这个人的博客，上面讲得很清楚了：https://blog.csdn.net/xiaoma_bk/article/details/79082629
这里就太数学了，但是没有关系已经有人这特好了。是c++ 的代码，只需都替换成golang 的函数就可以了。该有的函数都有，只需要使用 math 包进行替换就可以了。同时首字母大写。
c++ 代码，只需要把相关代码转换成golang 就可以了：
#define _USE_MATH_DEFINES#includestruct Quaternion {double w, x, y, z;}; struct EulerAngles {double roll, pitch, yaw;}; EulerAngles ToEulerAngles(Quaternion q) {EulerAngles angles; // roll (x-axis rotation)double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);double cosr_cosp = 1 – 2 * (q.x * q.x + q.y * q.y);angles.roll = std::atan2(sinr_cosp, cosr_cosp); // pitch (y-axis rotation)double sinp = 2 * (q.w * q.y – q.z * q.x);if (std::abs(sinp) >= 1)angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of rangeelseangles.pitch = std::asin(sinp); // yaw (z-axis rotation)double siny_cosp = 2 * (q.w * q.z + q.x * q.y);double cosy_cosp = 1 – 2 * (q.y * q.y + q.z * q.z);angles.yaw = std::atan2(siny_cosp, cosy_cosp); return angles;}
代码如下，写个 main 函数测试下：
package mainimport (“math”)/**将里程信息 w x y z 转换成欧拉坐标。*/func quaternionToEuler(w, x, y, z float64) {var roll, pitch, yaw float64// roll (x-axis rotation)var sinr_cosp float64 = 2 * (w*x + y*z)var cosr_cosp float64 = 1 – 2*(x*x+y*y)roll = math.Atan2(sinr_cosp, cosr_cosp)// pitch (y-axis rotation)var sinp float64 = 2 * (w*y – z*x)if math.Abs(sinp) >= 1 {pitch = math.Copysign(math.Pi/2, sinp) // use 90 degrees if out of range} else {pitch = math.Asin(sinp)}// yaw (z-axis rotation)var siny_cosp float64 = 2 * (w*z + x*y)var cosy_cosp float64 = 1 – 2*(y*y+z*z)yaw = math.Atan2(siny_cosp, cosy_cosp)println(roll)println(pitch)println(yaw)}func main() {// create a node and connect to the masterquaternionToEuler(2.3, 1.2, 0.1, 5.0)}4，总结
只要找到相关的对应方法，就可以把消息正确地解析出来，各种的消息信息 goroslib 已经都有了，只需要找到相关的消息类，然后订阅下就可以。通过里程消息后，再进行转换就可以变成 欧拉坐牢了。接下来就可以处理了。
