【无人水面艇路径跟随控制4】（C++）USV代码阅读： ROS节点订阅目标点消息，并将接收到的目标点添加到路径中，同时发布路径消息和可视化标记消息
【无人水面艇路径跟随控制4】（C++）USV代码阅读： ROS节点订阅目标点消息，并将接收到的目标点添加到路径中，同时发布路径消息和可视化标记消息
写在最前面
阅读代码：https://github.com/USE-jx/USV_path_follow/tree/main
usv path follow：无人水面艇路径跟随
trajectory tracking：轨迹跟踪
publish_waypoints.cpp
motion_control\guidance\publish_waypoints\src\publish_waypoints.cpp
这个文件 publish_waypoints.cpp 是一个ROS节点，用于接收目标点并发布路径点和可视化标记。
总结
这个ROS节点订阅目标点消息，并将接收到的目标点添加到路径中，同时发布路径消息和可视化标记消息。通过这种方式，可以在ROS环境中实现路径点的记录和可视化。
代码解释
以下是代码的详细解释：
头文件包含
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
ros/ros.h：包含ROS的基本功能。nav_msgs/Path.h：包含路径消息类型。geometry_msgs/PoseStamped.h：包含带时间戳的位姿消息类型。visualization_msgs/Marker.h：包含可视化标记消息类型。
全局变量声明
ros::Subscriber goal_sub;
ros::Publisher waypoints_pub;
ros::Publisher point_marker_pub;
nav_msgs::Path waypoints_msg;
visualization_msgs::Marker marker;
goal_sub：订阅器，用于接收目标点。waypoints_pub：发布器，用于发布路径点。point_marker_pub：发布器，用于发布可视化标记。waypoints_msg：路径消息，用于存储路径点。marker：标记消息，用于存储可视化标记。
回调函数
void goalCallback(const geometry_msgs::PoseStampedConstPtr &goal) {
waypoints_msg.header.frame_id = "map";
waypoints_msg.header.stamp = ros::Time::now();
waypoints_msg.poses.push_back(*goal);
waypoints_pub.publish(waypoints_msg);
marker.header.frame_id = "map";
marker.header.stamp = ros::Time::now();
marker.ns = "basic_shapes";
marker.id = 0;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.x = 0.3;
marker.scale.y = 0.3;
marker.scale.z = 0.3;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 1.0;
marker.color.a = 1.0;
marker.lifetime = ros::Duration();
geometry_msgs::Point pt;
pt.x = goal->pose.position.x;
pt.y = goal->pose.position.y;
pt.z = goal->pose.position.z;
marker.points.push_back(pt);
point_marker_pub.publish(marker);
}
goalCallback：回调函数，当接收到目标点时调用。- 更新
waypoints_msg的头信息，并将接收到的目标点添加到路径中，然后发布路径消息。 - 更新
marker的头信息和其他属性（如命名空间、ID、类型、动作、缩放、颜色和生命周期）。 - 创建一个
geometry_msgs::Point对象，并将目标点的位置信息赋值给它。 - 将该点添加到
marker的点列表中，然后发布标记消息。
- 更新
主函数
int main(int argc, char **argv)
{
ros::init(argc, argv, "waypoints_publisher");
main函数：程序的入口点。ros::init(argc, argv, "waypoints_publisher")：初始化ROS节点，节点名称为waypoints_publisher。
hello，我是 是Yu欸 。如果你喜欢我的文章，欢迎三连给我鼓励和支持：👍点赞 📁 关注 💬评论，我会给大家带来更多有用有趣的文章。
原文链接 👉 ，⚡️更新更及时。
欢迎大家添加好友交流。
原文地址：https://blog.csdn.net/wtyuong/article/details/142716769
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！