使用goroslib库，订阅ROS消息并解析，找到相关的里程计Odometry类，并成功解析消息，使用golang实现四元数到欧拉角的转换

Posted 2022-10-13 freewebsys

目录

• 前言
• • 1，ROS坐标系、单位的相关概念
  • 2，接入ROS 的topic 后一定要找好对应的消息类，才可以成功解析
  • 3，找到里程信息之后需要，把四元数到欧拉角
  • 4，总结

前言

---

本文的原文连接是:
https://blog.csdn.net/freewebsys/article/details/127272364

未经博主允许不得转载。

博主CSDN地址是：https://blog.csdn.net/freewebsys
博主掘金地址是：https://juejin.cn/user/585379920479288
博主知乎地址是：https://www.zhihu.com/people/freewebsystem

1，ROS坐标系、单位的相关概念

首先要补充下 ROS 里面的基本概念。
ROS 使用的是 C++编写的，但是因为特殊需要使用golang 进行编程，然后链接到 ROS的服务上。

所以要知道 ROS 里面的消息的结构体，就可以正确的进行解析了。

ROS里有统一定义了一系列机器人系统常用的坐标系和单位。

Point(点)

# This contains the position of a point in free space
float64 x
float64 y
float64 z

Quaternion(四元数)

# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w

Pose(姿态)

# A representation of pose in free space, composed of position and orientation. 
Point position
Quaternion orientation

Twist

# This expresses velocity in free space broken into its linear and angular parts.
Vector3 linear
Vector3 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_id
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_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,lll
package std_msgs

import (
	"github.com/aler9/goroslib/pkg/msg"
)

type String struct 
	msg.Package `ros:"std_msgs"`
	Data        string

再比如，里程 类：
https://pkg.go.dev/github.com/aler9/goroslib/pkg/msgs/nav_msgs#Odometry

//autogenerated:yes
//nolint:revive,lll
package nav_msgs

import (
	"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.Header
	ChildFrameId string
	Pose         geometry_msgs.PoseWithCovariance
	Twist        geometry_msgs.TwistWithCovariance

然后就可以按照这个类的数据进行解析了。

3，找到里程信息之后需要，把四元数到欧拉角

参考这个人的博客，上面讲的很清楚了：
https://blog.csdn.net/xiaoma_bk/article/details/79082629

这里就太数学了，但是没有关系已经有人这特好了。
是c++ 的代码，只需都替换成golang 的函数就可以了。
该有的函数都有，只需要使用 math 包进行替换就可以了。同时首字母大写。

c++ 代码，只需要把相关代码转换成golang 就可以了：

#define _USE_MATH_DEFINES
#include <cmath>
 
struct 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 range
    else
        angles.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 main

import (
	"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 master
	quaternionToEuler(2.3, 1.2, 0.1, 5.0)

4，总结

只要找到先关的对应方法，就可以把消息正确的解析出来，各种的消息信息 goroslib 已经都有了，只需要找到相关的消息类，然后订阅下就可以。
通过里程消息后，再进行转换就可以变成 欧拉坐角了。
接下来就可以处理了。

本文的原文连接是:
https://blog.csdn.net/freewebsys/article/details/127272364