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

写在最前面

阅读代码：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);

• getCourseFromWaypoints 函数从路径点和里程计信息中计算期望航向。
• 如果路径点数量少于2个，返回当前航向和速度0。
• 获取当前位置信息 current_x 和 current_y。
• 获取当前路径点和下一个路径点的坐标。
• 计算路径切线角 pi_p。

hello，我是 是Yu欸 。如果你喜欢我的文章，欢迎三连给我鼓励和支持：👍点赞 📁 关注 💬评论，我会给大家带来更多有用有趣的文章。
原文链接 👉 ，⚡️更新更及时。

欢迎大家添加好友交流。