四轮独转底盘轮式里程计实现记录
里程计流程与模式调度：ranger_messenger.cpp
各运动模式的运动学模型定义：kinematics_model.hpp
第一部分：部署环境
本文记录一套适用于 ROS2 Humble 的四轮独立驱动、四轮独立转向底盘轮式里程计算法。
算法运行环境：
- Ubuntu 22.04
- ROS2 Humble
- 四轮独立驱动
- 四轮独立转向
- 二维平面运动
- 输入为四个轮子的实际线速度与实际转向角
- 输出为二维轮式里程计
底盘采用 ROS 常用坐标系：
+x：车体前方+y：车体左方+z：车体上方- yaw 逆时针方向为正
四个轮子定义为：
+x
↑
FL ---------------- FR
| |
| base_link |
| O |
| |
RL ---------------- RR
← +y
其中：
FL：左前轮FR：右前轮RL：左后轮RR：右后轮
定义轴距为：
\[L
\]
定义轮距为：
\[W
\]
如果 base_link 位于四轮几何中心，则四个轮子相对于 base_link 的位置分别为：
\[\mathbf{r}_{FL}
=
\begin{bmatrix}
L/2 \\
W/2
\end{bmatrix}
\]
\[\mathbf{r}_{FR}
=
\begin{bmatrix}
L/2 \\
-W/2
\end{bmatrix}
\]
\[\mathbf{r}_{RL}
=
\begin{bmatrix}
-L/2 \\
W/2
\end{bmatrix}
\]
\[\mathbf{r}_{RR}
=
\begin{bmatrix}
-L/2 \\
-W/2
\end{bmatrix}
\]
每个计算周期需要获取四个轮子的实际线速度：
\[v_{FL},\quad
v_{FR},\quad
v_{RL},\quad
v_{RR}
\]
以及四个轮子的实际转向角：
\[\delta_{FL},\quad
\delta_{FR},\quad
\delta_{RL},\quad
\delta_{RR}
\]
其中：
- 轮速单位为
m/s - 转向角单位为
rad - 轮速必须带正负号
- 转向角必须使用实际反馈值，而不是控制器下发的目标值
本文将底盘运动划分为以下模式：
1：平移
2：双阿克曼
3：原地旋转
4：停止
5：只转向不行走
7：边调整转向边平移
第二部分：轮式里程计格式和发布实现
ROS2 中使用：
nav_msgs/msg/Odometry
发布二维轮式里程计。
Odometry 消息中主要使用以下字段：
header.frame_id
child_frame_id
pose.pose.position.x
pose.pose.position.y
pose.pose.orientation
twist.twist.linear.x
twist.twist.linear.y
twist.twist.angular.z
推荐设置：
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
其中：
pose表示机器人相对于odom坐标系的累计位姿twist表示机器人在base_link坐标系下的当前速度
二维轮式里程计状态为：
\[\mathbf{x}
=
\begin{bmatrix}
x \\
y \\
\theta
\end{bmatrix}
\]
其中：
- \(x\)：机器人在
odom坐标系中的 x 位置 - \(y\)：机器人在
odom坐标系中的 y 位置 - \(\theta\)：机器人 yaw
车体速度为：
\[\mathbf{v}
=
\begin{bmatrix}
v_x \\
v_y \\
\omega_z
\end{bmatrix}
\]
其中：
- \(v_x\)：车体前向速度
- \(v_y\)：车体左向速度
- \(\omega_z\)：yaw 角速度
发布位置：
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
二维 yaw 转为四元数：
\[q_z=\sin\left(\frac{\theta}{2}\right)
\]
\[q_w=\cos\left(\frac{\theta}{2}\right)
\]
对应代码：
odom.pose.pose.orientation.x = 0.0;
odom.pose.pose.orientation.y = 0.0;
odom.pose.pose.orientation.z = std::sin(yaw * 0.5);
odom.pose.pose.orientation.w = std::cos(yaw * 0.5);
发布实际车体速度：
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = wz;
一个完整的 Odometry 发布函数可以写为：
void publishOdometry(
const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr& publisher,
const rclcpp::Time& stamp,
double x,
double y,
double yaw,
double vx,
double vy,
double wz)
{
nav_msgs::msg::Odometry odom;
odom.header.stamp = stamp;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation.x = 0.0;
odom.pose.pose.orientation.y = 0.0;
odom.pose.pose.orientation.z = std::sin(yaw * 0.5);
odom.pose.pose.orientation.w = std::cos(yaw * 0.5);
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = wz;
publisher->publish(odom);
}
第三部分：具体轮式里程计算法
3.1 平移模式
平移模式下，四个轮子共同产生车体二维平移。
第 \(i\) 个轮子的实际速度向量为：
\[\mathbf{u}_i
=
v_i
\begin{bmatrix}
\cos\delta_i \\
\sin\delta_i
\end{bmatrix}
\]
因此该轮在车体 x、y 方向上的速度分量为：
\[u_{i,x}=v_i\cos\delta_i
\]
\[u_{i,y}=v_i\sin\delta_i
\]
四轮共同估计车体平移速度：
\[v_x
=
\frac{1}{4}
\sum_{i=1}^{4}
v_i\cos\delta_i
\]
\[v_y
=
\frac{1}{4}
\sum_{i=1}^{4}
v_i\sin\delta_i
\]
平移模式不主动产生车体旋转，因此：
\[\omega_z=0
\]
对应代码：
void solveTranslation(
const std::array<double, 4>& wheel_speed,
const std::array<double, 4>& steering,
double& vx,
double& vy,
double& wz)
{
vx = 0.0;
vy = 0.0;
for (std::size_t i = 0; i < 4; ++i)
{
vx += wheel_speed[i] * std::cos(steering[i]);
vy += wheel_speed[i] * std::sin(steering[i]);
}
vx *= 0.25;
vy *= 0.25;
wz = 0.0;
}
代码中每个轮子的实际速度先根据实际转向角投影到车体 x、y 方向，然后对四个轮子的结果求平均。
3.2 双阿克曼模式
双阿克曼模式下，前后轮反向转向，车辆绕瞬时旋转中心运动。
首先由四轮实际滚动速度得到等效车速：
\[v
=
\frac{
v_{FL}
+
v_{FR}
+
v_{RL}
+
v_{RR}
}{4}
\]
对应代码：
const double v =
0.25 *
(
wheel_speed[FL]
+ wheel_speed[FR]
+ wheel_speed[RL]
+ wheel_speed[RR]
);
阿克曼转向时内侧轮和外侧轮转角不同，因此先将实际内侧轮角转换为车辆中心等效转角。
若内侧轮实际转角为 \(\phi_i\)，车辆中心等效转角为：
\[\phi_c
=
\operatorname{atan2}
\left(
L\sin\phi_i,\,
L\cos\phi_i
+
W\sin\phi_i
\right)
\]
对应代码：
double innerToCentral(
double inner_angle,
double wheelbase,
double track)
{
return std::atan2(
wheelbase * std::sin(inner_angle),
wheelbase * std::cos(inner_angle)
+ track * std::sin(inner_angle));
}
前后轮为反相转向，所以将后轮转角符号统一到前轮方向后，再分别计算前轴和后轴的等效中心转角：
\[\phi
=
\frac{
\phi_{front}
+
\phi_{rear}
}{2}
\]
得到车辆中心等效转角后，车体速度为：
\[v_x=v\cos\phi
\]
\[v_y=0
\]
双阿克曼车体角速度为：
\[\omega_z
=
\frac{
2v\sin\phi
}{L}
\]
对应代码：
vx = v * std::cos(phi);
vy = 0.0;
if (std::abs(wheelbase) > 1e-9)
{
wz =
2.0
* v
* std::sin(phi)
/ wheelbase;
}
else
{
wz = 0.0;
}
完整的双阿克曼计算函数为：
void solveDualAckermann(
const std::array<double, 4>& wheel_speed,
const std::array<double, 4>& steering,
double wheelbase,
double track,
double& vx,
double& vy,
double& wz)
{
constexpr std::size_t FL = 0;
constexpr std::size_t FR = 1;
constexpr std::size_t RL = 2;
constexpr std::size_t RR = 3;
const double v =
0.25 *
(
wheel_speed[FL]
+ wheel_speed[FR]
+ wheel_speed[RL]
+ wheel_speed[RR]
);
const double front_mean =
0.5 *
(
steering[FL]
+ steering[FR]
);
if (std::abs(front_mean) < 1e-8)
{
vx = v;
vy = 0.0;
wz = 0.0;
return;
}
double front_inner = 0.0;
double rear_inner_same_sign = 0.0;
if (front_mean > 0.0)
{
front_inner = steering[FL];
rear_inner_same_sign = -steering[RL];
}
else
{
front_inner = steering[FR];
rear_inner_same_sign = -steering[RR];
}
const double phi_front =
innerToCentral(
front_inner,
wheelbase,
track);
const double phi_rear =
innerToCentral(
rear_inner_same_sign,
wheelbase,
track);
const double phi =
0.5 *
(
phi_front
+ phi_rear
);
vx = v * std::cos(phi);
vy = 0.0;
if (std::abs(wheelbase) > 1e-9)
{
wz =
2.0
* v
* std::sin(phi)
/ wheelbase;
}
else
{
wz = 0.0;
}
}
3.3 原地旋转模式
原地旋转时车体中心没有平移：
\[v_x=0
\]
\[v_y=0
\]
只需要估计：
\[\omega_z
\]
第 \(i\) 个轮子相对于车体中心的位置为：
\[\mathbf{r}_i
=
\begin{bmatrix}
x_i \\
y_i
\end{bmatrix}
\]
轮子的实际速度向量为：
\[\mathbf{u}_i
=
\begin{bmatrix}
v_i\cos\delta_i \\
v_i\sin\delta_i
\end{bmatrix}
\]
纯旋转刚体运动满足：
\[\mathbf{u}_i
=
\omega_z
\begin{bmatrix}
-y_i \\
x_i
\end{bmatrix}
\]
因此由单个轮子可以估计：
\[\omega_i
=
\frac{
-y_i v_i\cos\delta_i
+
x_i v_i\sin\delta_i
}{
x_i^2+y_i^2
}
\]
使用四个轮子的估计值求平均：
\[\omega_z
=
\frac{
\omega_{FL}
+
\omega_{FR}
+
\omega_{RL}
+
\omega_{RR}
}{4}
\]
对应代码：
void solveSpinning(
const std::array<double, 4>& wheel_speed,
const std::array<double, 4>& steering,
const std::array<double, 4>& wheel_x,
const std::array<double, 4>& wheel_y,
double& vx,
double& vy,
double& wz)
{
double omega_sum = 0.0;
std::size_t valid_count = 0;
for (std::size_t i = 0; i < 4; ++i)
{
const double ux =
wheel_speed[i]
* std::cos(steering[i]);
const double uy =
wheel_speed[i]
* std::sin(steering[i]);
const double r2 =
wheel_x[i] * wheel_x[i]
+ wheel_y[i] * wheel_y[i];
if (r2 <= 1e-12)
{
continue;
}
const double omega_i =
(
-wheel_y[i] * ux
+ wheel_x[i] * uy
)
/ r2;
omega_sum += omega_i;
++valid_count;
}
vx = 0.0;
vy = 0.0;
if (valid_count > 0)
{
wz =
omega_sum
/ static_cast<double>(valid_count);
}
else
{
wz = 0.0;
}
}
3.4 停止与只转向模式
当底盘停止，或者只调整转向轮而驱动轮不行走时，车体不产生二维位移和 yaw 变化。
因此：
\[v_x=0
\]
\[v_y=0
\]
\[\omega_z=0
\]
对应代码：
vx = 0.0;
vy = 0.0;
wz = 0.0;
3.5 边调整转向边平移模式
如果该模式的物理定义是四个轮子在调整共同滚动方向的同时进行平移，而车体不主动改变 yaw，则采用与平移模式相同的计算：
\[v_x
=
\frac{1}{4}
\sum_{i=1}^{4}
v_i\cos\delta_i
\]
\[v_y
=
\frac{1}{4}
\sum_{i=1}^{4}
v_i\sin\delta_i
\]
\[\omega_z=0
\]
对应代码仍调用：
solveTranslation(
wheel_speed,
steering,
vx,
vy,
wz);
3.6 位姿积分
前面的运动学计算得到的是 base_link 坐标系下的：
\[v_x,\quad v_y,\quad \omega_z
\]
当前机器人在 odom 坐标系中的位姿为：
\[x,\quad y,\quad \theta
\]
则机器人在 odom 坐标系中的运动微分方程为：
\[\dot{x}
=
v_x\cos\theta
-
v_y\sin\theta
\]
\[\dot{y}
=
v_x\sin\theta
+
v_y\cos\theta
\]
\[\dot{\theta}
=
\omega_z
\]
写成矩阵形式：
\[\frac{d}{dt}
\begin{bmatrix}
x \\
y \\
\theta
\end{bmatrix}
=
\begin{bmatrix}
v_x\cos\theta-v_y\sin\theta \\
v_x\sin\theta+v_y\cos\theta \\
\omega_z
\end{bmatrix}
\]
使用四阶 Runge-Kutta 方法进行积分。
定义：
\[\mathbf{s}
=
\begin{bmatrix}
x \\
y \\
\theta
\end{bmatrix}
\]
则：
\[\dot{\mathbf{s}}
=
f(\mathbf{s})
\]
RK4 计算：
\[k_1=f(\mathbf{s}_k)
\]
\[k_2=
f\left(
\mathbf{s}_k
+
\frac{\Delta t}{2}k_1
\right)
\]
\[k_3=
f\left(
\mathbf{s}_k
+
\frac{\Delta t}{2}k_2
\right)
\]
\[k_4=
f\left(
\mathbf{s}_k
+
\Delta t\,k_3
\right)
\]
最终：
\[\mathbf{s}_{k+1}
=
\mathbf{s}_k
+
\frac{\Delta t}{6}
\left(
k_1+2k_2+2k_3+k_4
\right)
\]
对应的状态微分函数：
Derivative derivative(
double yaw,
double vx,
double vy,
double wz)
{
Derivative d;
d.dx =
vx * std::cos(yaw)
- vy * std::sin(yaw);
d.dy =
vx * std::sin(yaw)
+ vy * std::cos(yaw);
d.dyaw = wz;
return d;
}
RK4 积分代码：
void integrateRK4(
double vx,
double vy,
double wz,
double dt)
{
constexpr int substeps = 10;
const double h =
dt / static_cast<double>(substeps);
for (int i = 0; i < substeps; ++i)
{
const Derivative k1 =
derivative(yaw_, vx, vy, wz);
const Derivative k2 =
derivative(
yaw_
+ 0.5 * h * k1.dyaw,
vx,
vy,
wz);
const Derivative k3 =
derivative(
yaw_
+ 0.5 * h * k2.dyaw,
vx,
vy,
wz);
const Derivative k4 =
derivative(
yaw_
+ h * k3.dyaw,
vx,
vy,
wz);
x_ +=
h *
(
k1.dx
+ 2.0 * k2.dx
+ 2.0 * k3.dx
+ k4.dx
) / 6.0;
y_ +=
h *
(
k1.dy
+ 2.0 * k2.dy
+ 2.0 * k3.dy
+ k4.dy
) / 6.0;
yaw_ +=
h *
(
k1.dyaw
+ 2.0 * k2.dyaw
+ 2.0 * k3.dyaw
+ k4.dyaw
) / 6.0;
yaw_ =
std::atan2(
std::sin(yaw_),
std::cos(yaw_));
}
}
第四部分：完整源代码
下面是一份完整、独立的 ROS2 Humble C++ 示例。代码不依赖具体底盘驱动，四轮实际速度、四轮实际转向角以及运动模式通过代码中的接口传入。
#include <array>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <functional>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/odometry.hpp"
class FourWheelSteeringOdometry
{
public:
enum class MotionMode
{
STOP = 0,
TRANSLATION = 1,
DUAL_ACKERMANN = 2,
SPINNING = 3,
STEER_ONLY = 5,
TRANSLATION_WITH_STEERING = 7
};
struct State
{
double x{0.0};
double y{0.0};
double yaw{0.0};
double vx{0.0};
double vy{0.0};
double wz{0.0};
};
FourWheelSteeringOdometry(
double wheelbase,
double track)
: wheelbase_(wheelbase),
track_(track)
{
const double half_l =
0.5 * wheelbase_;
const double half_w =
0.5 * track_;
wheel_x_ =
{
+half_l,
+half_l,
-half_l,
-half_l
};
wheel_y_ =
{
+half_w,
-half_w,
+half_w,
-half_w
};
}
void reset(
double x = 0.0,
double y = 0.0,
double yaw = 0.0)
{
state_.x = x;
state_.y = y;
state_.yaw = normalizeAngle(yaw);
state_.vx = 0.0;
state_.vy = 0.0;
state_.wz = 0.0;
}
void update(
MotionMode mode,
const std::array<double, 4>& wheel_speed,
const std::array<double, 4>& steering,
double dt)
{
if (dt <= 0.0)
{
return;
}
double vx = 0.0;
double vy = 0.0;
double wz = 0.0;
switch (mode)
{
case MotionMode::TRANSLATION:
case MotionMode::TRANSLATION_WITH_STEERING:
solveTranslation(
wheel_speed,
steering,
vx,
vy,
wz);
break;
case MotionMode::DUAL_ACKERMANN:
solveDualAckermann(
wheel_speed,
steering,
vx,
vy,
wz);
break;
case MotionMode::SPINNING:
solveSpinning(
wheel_speed,
steering,
vx,
vy,
wz);
break;
case MotionMode::STOP:
case MotionMode::STEER_ONLY:
default:
vx = 0.0;
vy = 0.0;
wz = 0.0;
break;
}
state_.vx = vx;
state_.vy = vy;
state_.wz = wz;
integrateRK4(
vx,
vy,
wz,
dt);
}
const State& state() const
{
return state_;
}
private:
static constexpr std::size_t FL = 0;
static constexpr std::size_t FR = 1;
static constexpr std::size_t RL = 2;
static constexpr std::size_t RR = 3;
struct Derivative
{
double dx;
double dy;
double dyaw;
};
double wheelbase_;
double track_;
std::array<double, 4> wheel_x_;
std::array<double, 4> wheel_y_;
State state_;
static double normalizeAngle(
double angle)
{
return std::atan2(
std::sin(angle),
std::cos(angle));
}
double innerToCentral(
double inner_angle) const
{
return std::atan2(
wheelbase_
* std::sin(inner_angle),
wheelbase_
* std::cos(inner_angle)
+ track_
* std::sin(inner_angle));
}
void solveTranslation(
const std::array<double, 4>& wheel_speed,
const std::array<double, 4>& steering,
double& vx,
double& vy,
double& wz) const
{
vx = 0.0;
vy = 0.0;
for (std::size_t i = 0;
i < 4;
++i)
{
vx +=
wheel_speed[i]
* std::cos(steering[i]);
vy +=
wheel_speed[i]
* std::sin(steering[i]);
}
vx *= 0.25;
vy *= 0.25;
wz = 0.0;
}
void solveDualAckermann(
const std::array<double, 4>& wheel_speed,
const std::array<double, 4>& steering,
double& vx,
double& vy,
double& wz) const
{
const double v =
0.25 *
(
wheel_speed[FL]
+ wheel_speed[FR]
+ wheel_speed[RL]
+ wheel_speed[RR]
);
const double front_mean =
0.5 *
(
steering[FL]
+ steering[FR]
);
if (std::abs(front_mean) < 1e-8)
{
vx = v;
vy = 0.0;
wz = 0.0;
return;
}
double front_inner = 0.0;
double rear_inner_same_sign = 0.0;
if (front_mean > 0.0)
{
front_inner =
steering[FL];
rear_inner_same_sign =
-steering[RL];
}
else
{
front_inner =
steering[FR];
rear_inner_same_sign =
-steering[RR];
}
const double phi_front =
innerToCentral(
front_inner);
const double phi_rear =
innerToCentral(
rear_inner_same_sign);
const double phi =
0.5 *
(
phi_front
+ phi_rear
);
vx =
v * std::cos(phi);
vy = 0.0;
if (std::abs(wheelbase_) > 1e-9)
{
wz =
2.0
* v
* std::sin(phi)
/ wheelbase_;
}
else
{
wz = 0.0;
}
}
void solveSpinning(
const std::array<double, 4>& wheel_speed,
const std::array<double, 4>& steering,
double& vx,
double& vy,
double& wz) const
{
double omega_sum = 0.0;
std::size_t valid_count = 0;
for (std::size_t i = 0;
i < 4;
++i)
{
const double ux =
wheel_speed[i]
* std::cos(steering[i]);
const double uy =
wheel_speed[i]
* std::sin(steering[i]);
const double r2 =
wheel_x_[i]
* wheel_x_[i]
+ wheel_y_[i]
* wheel_y_[i];
if (r2 <= 1e-12)
{
continue;
}
const double omega_i =
(
-wheel_y_[i] * ux
+ wheel_x_[i] * uy
)
/ r2;
omega_sum += omega_i;
++valid_count;
}
vx = 0.0;
vy = 0.0;
if (valid_count > 0)
{
wz =
omega_sum
/ static_cast<double>(
valid_count);
}
else
{
wz = 0.0;
}
}
static Derivative derivative(
double yaw,
double vx,
double vy,
double wz)
{
Derivative d;
d.dx =
vx * std::cos(yaw)
- vy * std::sin(yaw);
d.dy =
vx * std::sin(yaw)
+ vy * std::cos(yaw);
d.dyaw = wz;
return d;
}
void integrateRK4(
double vx,
double vy,
double wz,
double dt)
{
constexpr int substeps = 10;
const double h =
dt
/ static_cast<double>(
substeps);
for (int i = 0;
i < substeps;
++i)
{
const Derivative k1 =
derivative(
state_.yaw,
vx,
vy,
wz);
const Derivative k2 =
derivative(
state_.yaw
+ 0.5
* h
* k1.dyaw,
vx,
vy,
wz);
const Derivative k3 =
derivative(
state_.yaw
+ 0.5
* h
* k2.dyaw,
vx,
vy,
wz);
const Derivative k4 =
derivative(
state_.yaw
+ h
* k3.dyaw,
vx,
vy,
wz);
state_.x +=
h *
(
k1.dx
+ 2.0 * k2.dx
+ 2.0 * k3.dx
+ k4.dx
)
/ 6.0;
state_.y +=
h *
(
k1.dy
+ 2.0 * k2.dy
+ 2.0 * k3.dy
+ k4.dy
)
/ 6.0;
state_.yaw +=
h *
(
k1.dyaw
+ 2.0 * k2.dyaw
+ 2.0 * k3.dyaw
+ k4.dyaw
)
/ 6.0;
state_.yaw =
normalizeAngle(
state_.yaw);
}
}
};
class WheelOdometryNode : public rclcpp::Node
{
public:
WheelOdometryNode()
: Node("wheel_odometry"),
odometry_(
0.500,
0.480)
{
odom_pub_ =
create_publisher<
nav_msgs::msg::Odometry>(
"/wheel_odom",
10);
last_time_ = now();
timer_ =
create_wall_timer(
std::chrono::milliseconds(20),
std::bind(
&WheelOdometryNode::update,
this));
}
private:
void update()
{
const rclcpp::Time current_time =
now();
const double dt =
(
current_time
- last_time_
).seconds();
last_time_ =
current_time;
/*
* 下面三个变量在实际底盘中应由
* 底盘反馈接口更新。
*
* wheel_speed:
* FL, FR, RL, RR
*
* steering:
* FL, FR, RL, RR
*
* mode:
* 当前运动模式
*/
std::array<double, 4> wheel_speed =
{
0.0,
0.0,
0.0,
0.0
};
std::array<double, 4> steering =
{
0.0,
0.0,
0.0,
0.0
};
FourWheelSteeringOdometry::MotionMode mode =
FourWheelSteeringOdometry::MotionMode::STOP;
odometry_.update(
mode,
wheel_speed,
steering,
dt);
publishOdometry(
current_time);
}
void publishOdometry(
const rclcpp::Time& stamp)
{
const auto& state =
odometry_.state();
nav_msgs::msg::Odometry odom;
odom.header.stamp =
stamp;
odom.header.frame_id =
"odom";
odom.child_frame_id =
"base_link";
odom.pose.pose.position.x =
state.x;
odom.pose.pose.position.y =
state.y;
odom.pose.pose.position.z =
0.0;
odom.pose.pose.orientation.x =
0.0;
odom.pose.pose.orientation.y =
0.0;
odom.pose.pose.orientation.z =
std::sin(
state.yaw * 0.5);
odom.pose.pose.orientation.w =
std::cos(
state.yaw * 0.5);
odom.twist.twist.linear.x =
state.vx;
odom.twist.twist.linear.y =
state.vy;
odom.twist.twist.angular.z =
state.wz;
odom_pub_->publish(
odom);
}
FourWheelSteeringOdometry odometry_;
rclcpp::Publisher<
nav_msgs::msg::Odometry
>::SharedPtr odom_pub_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time last_time_;
};
int main(
int argc,
char** argv)
{
rclcpp::init(
argc,
argv);
rclcpp::spin(
std::make_shared<
WheelOdometryNode>());
rclcpp::shutdown();
return 0;
}
浙公网安备 33010602011771号