当前位置：首页 > article >正文

【无人水面艇路径跟随控制2】（C++）USV代码阅读： SetOfLos 类的从路径点和里程计信息中计算期望航向

article 2024/10/7 3:29:08

【无人水面艇路径跟随控制2】（C++）USV代码阅读： SetOfLos 类的从路径点和里程计信息中计算期望航向

写在最前面
set_of_los.cpp
- 小结
- 详细解释
- - 头文件包含
  - 命名空间
  - 构造函数和析构函数
  - 设置参数函数
  - 获取航向函数

请添加图片描述

🌈你好呀！我是是Yu欸 🌌 2024每日百字篆刻时光，感谢你的陪伴与支持 ~ 🚀 欢迎一起踏上探险之旅，挖掘无限可能，共同成长！

写在最前面

阅读代码：https://github.com/USE-jx/USV_path_follow/tree/main

usv path follow：无人水面艇路径跟随
trajectory tracking：轨迹跟踪

请添加图片描述

set_of_los.cpp

motion_control\guidance\los_guidance\src\set_of_los.cpp

这个文件 set_of_los.cpp 实现了 set_of_los.h 中定义的 SetOfLos 类的成员函数。

小结

这个代码片段实现了 SetOfLos 类的构造函数、析构函数、参数设置函数和部分获取航向函数。通过这些函数，可以初始化对象、设置参数并从路径点和里程计信息中计算期望航向。

详细解释

以下是代码的详细解释：

头文件包含

#include "los_guidance/set_of_los.h"

包含头文件 set_of_los.h。

命名空间

namespace motion_control {

将代码放在 motion_control 命名空间中，以避免命名冲突。

构造函数和析构函数

SetOfLos::SetOfLos() : index_(0), desired_course_(0), desired_speed_(0), is_final_(false) {}

SetOfLos::~SetOfLos() {}

构造函数 SetOfLos() 初始化成员变量：
- index_：路径点索引，初始值为0。
- desired_course_：期望航向，初始值为0。
- desired_speed_：期望速度，初始值为0。
- is_final_：是否到达最终目标，初始值为false。
析构函数 ~SetOfLos()：目前没有需要清理的资源。

设置参数函数

void SetOfLos::setParam(ros::NodeHandle& nh) {
    nh.param("los/desired_speed", desired_speed_, 1.0);
    nh.param("los/switch_distance", switch_dist_, 7.0);
    nh.param("los/use_const_delta", use_const_delta_, true);
    nh.param("los/const_delta", const_delta_, 3.0);
    nh.param("los/delta_max", delta_max_, 10.0);
    nh.param("los/delta_min", delta_min_, 1.0);
    nh.param("los/gamma", gamma_, 1.0);
}

从ROS节点句柄中读取参数并设置成员变量：

desired_speed_：期望速度，默认值为1.0。
switch_dist_：切换路径点的距离，默认值为7.0。
use_const_delta_：是否使用常量前视距离，默认值为true。
const_delta_：常量前视距离，默认值为3.0。
delta_max_：最大前视距离，默认值为10.0。
delta_min_：最小前视距离，默认值为1.0。
gamma_：时间变化参数，默认值为1.0。

获取航向函数

Eigen::Vector2d SetOfLos::getCourseFromWaypoints(const nav_msgs::Path &path, 
                                        const nav_msgs::Odometry &odom,
                                        bool &is_final) {
    if (path.poses.size() < 2) {
        return {tf2::getYaw(odom.pose.pose.orientation), 0};
    }

    //current position
    double current_x = odom.pose.pose.position.x;
    double current_y = odom.pose.pose.position.y;

    //calculate path-tangential angle
    double waypoint_cur_x = path.poses[index_].pose.position.x;
    double waypoint_cur_y = path.poses[index_].pose.position.y;

    double waypoint_next_x = path.poses[index_+1].pose.position.x;
    double waypoint_next_y = path.poses[index_+1].pose.position.y;
    double pi_p = atan2(waypoint_next_y - waypoint_cur_y, waypoint_next_x - waypoint_cur_x);