【无人水面艇路径跟随控制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欸 。如果你喜欢我的文章,欢迎三连给我鼓励和支持:👍点赞 📁 关注 💬评论,我会给大家带来更多有用有趣的文章。
原文链接 👉 ,⚡️更新更及时。
欢迎大家添加好友交流。