【无人水面艇路径跟随控制2】(C++)USV代码阅读: SetOfLos 类的从路径点和里程计信息中计算期望航向
- 写在最前面
- set_of_los.cpp
- 小结
- 详细解释
- 头文件包含
- 命名空间
- 构造函数和析构函数
- 设置参数函数
- 获取航向函数
写在最前面
阅读代码: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 positiondouble current_x = odom.pose.pose.position.x;double current_y = odom.pose.pose.position.y;//calculate path-tangential angledouble 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欸 。如果你喜欢我的文章,欢迎三连给我鼓励和支持:👍点赞 📁 关注 💬评论,我会给大家带来更多有用有趣的文章。
原文链接 👉 ,⚡️更新更及时。
欢迎大家添加好友交流。