Rviz中publish point使用

原创已于 2023-04-29 12:25:33 修改 · 2.4k 阅读

13 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #开发语言

于 2023-03-25 14:02:56 首次发布

ROS 专栏收录该内容

2 篇文章

订阅专栏

本文介绍了如何在RViz中订阅并处理/clicked_point话题，以此实现多点巡航功能。通过创建订阅者和可视化marker发布者，每次点击RViz界面时会在对应位置显示marker，并记录点击次数。到达目标点后，通过另一个回调函数处理后续目标点的发布，从而完成多目标点导航任务。

我们知道当rviz运行以后，他会自己发布几个话题消息/clicked_point、/initialpose、/move_base_simple/goal等，而rviz图形界面上publish point发布出来的话题就是/clicked_point，因此我们需要做的就是订阅该话题并在回调中实现多点巡航。

在这之前需要了解rviz中marker可视化内容，因为我们需要看到publish point点在rviz中的什么位置以什么形状显示的，可以参考这篇文章使用visualization_msgs::Marker可视化，github上也有开源的项目rviz_visual_tools等可以参考。

首先创建一个/clicked_point的订阅者、marker可视化的发布者和geometry_msgs/posestamped的发布者，在回调中对marker进行处理，比如marker的形状，透明度等，同时每个marker要有独立的id。

mark_pub = nh.advertise<visualization_msgs::MarkerArray>("/path_point",100);
click_sub = nh.subscribe<geometry_msgs::PointStamped>("/clicked_point",100,click_callback);
goal_pub =nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal",100);

回调函数中这么处理，实现每点击一次publish point都会有一个marker图形在rviz相应位置同时记录点击的次数，这用于后续各目标点的顺序发布。

//订阅click_point点
void click_callback(const geometry_msgs::PointStamped::ConstPtr& msg)
{
    ROS_INFO("Add a new point %f",counts);
    ROS_INFO("x: %f ,y: %f ,z: %f ",msg->point.x,msg->point.y,msg->point.z);

    marker.header.frame_id = "map";
    // marker.header.stamp = ros::Time::now();
    marker.type = marker.ARROW; //一直面向屏幕的字符格式
    marker.action = marker.ADD; //添加marker
    marker.scale.x = 0.2;
    marker.scale.y = 0.05;
    marker.scale.z = 0.05;
    marker.color.a = 1; //透明度
    marker.color.b = 0;
    marker.color.g = 1;
    marker.color.r = 0;

    marker.pose.position.x = msg->point.x;
    marker.pose.position.y = msg->point.y;
    marker.pose.orientation.z = 0;
    marker.pose.orientation.w = 1;
    markerarray.markers.push_back(marker);

    marker.id +=1;
    mark_pub.publish(markerarray);

    if(counts==0)
    {
        geometry_msgs::PoseStamped pose;
        pose.header.frame_id ="map";
        pose.header.stamp = ros::Time::now();
        pose.pose.position.x = msg->point.x;
        pose.pose.position.y = msg->point.y;
        pose.pose.orientation.w = 1;
        pose.pose.orientation.z = 0;
        goal_pub.publish(pose);
        indexs+=1; //下一次要发布的目标点序号
    }
    counts+=1; //记录几个目标点

}

在这个回调中我们发布了一个目标点，那么我们到达该目标点之后该怎么操作呢，这里我们就需要再创建一个回调函数用于处理到达目标点的情况一个如何进行后续目标点的发布。

 goal_state_sub = nh.subscribe<mbf_msgs::MoveBaseActionResult>("/move_base/result",100,pose_callback);

void pose_callback(const mbf_msgs::MoveBaseActionResult::ConstPtr& msg)
{
    if(msg->status.SUCCEEDED == 3 && counts>0)
    {
        if(indexs == counts) 
        {
            ROS_INFO("reached the finial goal");
            if(counts > 1)
            {
                indexs = 0;
                geometry_msgs::PoseStamped pose;
                pose.header.frame_id = "map";
                pose.header.stamp = ros::Time::now();
                pose.pose.position.x = markerarray.markers[indexs].pose.position.x;
                pose.pose.position.y = markerarray.markers[indexs].pose.position.y;
                pose.pose.orientation.z = markerarray.markers[indexs].pose.orientation.z;
                pose.pose.orientation.w = markerarray.markers[indexs].pose.orientation.w;
                goal_pub.publish(pose);
                indexs+=1;
            }
        }
        else if(indexs < counts) 
        {
            geometry_msgs::PoseStamped pose;
            pose.header.frame_id = "map";
            // pose.header.stamp = ros::Time::now();
            pose.pose.position.x = markerarray.markers[indexs].pose.position.x;
            pose.pose.position.y = markerarray.markers[indexs].pose.position.y;
            pose.pose.orientation.z = markerarray.markers[indexs].pose.orientation.z;
            pose.pose.orientation.w = markerarray.markers[indexs].pose.orientation.w;
            goal_pub.publish(pose);
            indexs+=1;
        }
    }
}

这样就基本实现了利用publish point实现多目标点巡航，或者不需要多目标点巡航而只实现多目标点导航的可以在这基础上进行修改。

完结～