(九)ROS在rviz中实时显示轨迹(nav_msgs/Path消息的使用)

ROS 同时被 2 个专栏收录
28 篇文章 9 订阅

如何在rviz中如何实时显示轨迹呢?
本文分析nav_msgs/Path结构,实现在rviz中画出圆形轨迹。

1.使用参考代码画轨迹效果展示(hector_slam)

google rviz中如何实时显示轨迹?会有以下重要资料。
参考资料
参考http://answers.ros.org/question/209224/show-robot-trajectory-in-rviz-real-time/
hector_slam中实现了画轨迹。
网址http://wiki.ros.org/hector_slam/Tutorials/MappingUsingLoggedData
那么先用bag数据来跑一跑。
步骤如下:

(1)启动包

roslaunch hector_slam_launch tutorial.launch

(2)回放bag
rosbag play Team_Hector_MappingBox_RoboCup_2011_Rescue_Arena.bag --clock
(3)效果
这里写图片描述

(4)查看发送的topic
rostopic echo /trajectory

这里写图片描述

2.实现在rviz中画出圆形轨迹

以上的包太复杂,本文实现最简单的画轨迹功能。
##2.1分析nav_msgs/Path.msg结构
nav_msgs/Path.msg结构

std_msgs/Header header
geometry_msgs/PoseStamped[] poses

将以上结构展开如下:
std_msgs/Header结构

uint32 seq
time stamp
string frame_id

geometry_msgs/PoseStamped[]结构

std_msgs/Header header
geometry_msgs/Pose pose

geometry_msgs/Pose pose结构

# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w

2.2实现画出圆形轨迹

(1)新建工程

mkdir -p showpath/src
cd src
catkin_create_pkg showpath roscpp rospy sensor_msgs std_msgs nav_msgs tf
cd ..
catkin_make 

(2)编辑主函数showpath.cpp

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>

main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath");

    ros::NodeHandle ph;
    ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("trajectory",1, true);

    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    nav_msgs::Path path;
    //nav_msgs::Path path;
    path.header.stamp=current_time;
    path.header.frame_id="odom";


    double x = 0.0;
    double y = 0.0;
    double th = 0.0;
    double vx = 0.1;
    double vy = -0.1;
    double vth = 0.1;

    ros::Rate loop_rate(1);
    while (ros::ok())
    {

        current_time = ros::Time::now();
        //compute odometry in a typical way given the velocities of the robot
        double dt = 0.1;
        double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
        double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
        double delta_th = vth * dt;

        x += delta_x;
        y += delta_y;
        th += delta_th;


        geometry_msgs::PoseStamped this_pose_stamped;
        this_pose_stamped.pose.position.x = x;
        this_pose_stamped.pose.position.y = y;

        geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(th);
        this_pose_stamped.pose.orientation.x = goal_quat.x;
        this_pose_stamped.pose.orientation.y = goal_quat.y;
        this_pose_stamped.pose.orientation.z = goal_quat.z;
        this_pose_stamped.pose.orientation.w = goal_quat.w;

        this_pose_stamped.header.stamp=current_time;
        this_pose_stamped.header.frame_id="odom";
        path.poses.push_back(this_pose_stamped);

        path_pub.publish(path);
        ros::spinOnce();               // check for incoming messages

        last_time = current_time;
        loop_rate.sleep();
    }

    return 0;
}

(3)编辑CMakeLists.txt
在最后加入

add_executable(showpath src/showpath.cpp)
target_link_libraries(showpath ${catkin_LIBRARIES})

(4)编译和运行

编译

cd /home/topeet/workspace/showpath
catkin_make

运行

source ./devel/setup.bash
rosrun showpath showpath

查看/trajectory 信息

rostopic echo /trajectory

#3.rviz实验结果
运行

rosrun rviz rviz

在globel option的Fixed Fram输入odom
左边点击add
选中path
在path的topic选项中选
/trajectory

这里写图片描述

相关推荐
<p>      掌握基于腾讯人工智能AI车牌识别技术,使用车牌识别技术实现一个完整停车场管理系统,项目包括网页调用摄像头拍照,车牌拍照识别,上传车牌图片识别,用户管理,车辆管理临时车与包月车,车辆出场,入场管理,停车费收费管理,按照临时车或包月车自动计算停车费,系统参数设置,修改用户密码及安全退出等功能,该系统采用Jsp技术,使用SSM框架,Mysql数据库,ajax技术及人工智能等相关技术实现。</p> <h2><span style="color: #e03e2d;">重要通知:本课程根据腾讯AI车牌识别新接口,更新了新接口源代码,发布程序,购买了课程同学可以下载新程序,包括运行程序及源代码,更新时间:2021-2-17</span><br /><br /><span style="color: #e53333;">项目开发技术:java,jsp,mysql,MyBatis,SpringMVC,jquery,ajax,json</span><br /><span style="color: #e53333;">项目运行环境:jdk1.7及以上版本,tomcat6.0及以上版本,mysql5.5及以上版本</span><br /><span style="color: #e53333;">项目开发工具: 本项目开发工具是Eclipse,也支持myEclipse,Intellij Idea等其他版本开发工具</span><br /><br /></h2> <p style="color: #333333;"><span style="font-size: 20px;"><span style="color: #ff0000;"><strong>相关课程学习顺序</strong></span></span></p> <p style="color: #333333;">本校课程是培养JAVA软件工程师及JSP WEB网络应用程序开发,android工程师全套课程,课程学习顺序如下:<br /><span style="color: #ff0000;"><strong>JAVA初级工程师:</strong></span><br />    1、计算机基础<br />    2、HTML语言基础<br />    3、C语言从入门到精通+贪吃蛇游戏<br />    4、贪吃蛇游戏<br />    5、SQL SERVER数据库基础<br />    6、JAVA从入门到精通+推箱子游戏+QQ即时通讯软件<br />    7、推箱子游戏;<br />    8、仿QQ即时通讯软件;<br /><span style="color: #ff0000;"><strong>JAVA级工程师:</strong></span><br />    9、SQLSERVER数据库高级<br />    10、SQLSERVER从入门到精通基础+高级<br />              11、JavaScript从入门到精通,<br />    12、JSP从入门到精通+点餐系统,<br />    13、JSP从入门到精通+线视频学习教育平台,<br />    14、JSP从入门到精通+大型电商平台;<br />    15、XML从入门到精通,<br />    16、数据结构JAVA版,<br /><span style="color: #ff0000;"><strong>JAVA高级工程师:</strong></span><br />    17、Oracle数据库从入门到精通,<br />    18、ajax+jquery从入门到精通,<br />    19、EasyUI从入门到精通,<br /><span style="color: #ff0000;"><strong>SSH框架:</strong></span><br />    20、Struts2从入门到精通课程,<br />    21、Hibernate从入门到精通课程,<br />    22、Spring从入门到精通课程;<br />    23、Echarts从入门到精通,<br />    24、Excel基于POI导入导出<br /><span style="color: #ff0000;"><strong>工作流框架:</strong></span><br />    25、Activiti流程框架从入门到精通<br />    26、JBPM流程框架从入门到精通<br /><span style="color: #ff0000;"><strong>SSM框架:</strong></span><br />    27、MyBatis从入门到精通<br />    28、Spring MVC从入门到精通<br /><span style="color: #ff0000;"><strong>面试题:</strong></span><br />    29、职业生涯规划及面试题集锦<br /><span style="color: #ff0000;"><strong>商业项目:</strong></span><br />    30、微信公众号线支付系统<br />    31、微信生活缴费线支付系统<br />    32、支付宝生活缴费线支付系统<br />    33、线考试系统<br />    34、手机订餐管理系统,<br />    35、CRM客户关系管理系统<br />    36、大型房地产CRM销售管理系统<br />    37、CMPP2,CMPP3移动网关系统<br /><span style="color: #ff0000;"><strong>人工智能:</strong></span><br />    38、人脸识别线考试系统<br />    39、人脸识别系统项目实战<br />    40、车牌识别系统项目实战<br />    41、身份证识别系统项目实战<br />    42、营业执照识别系统项目实战</p> <p style="color: #333333;">          43、名片识别管理系统</p>
©️2020 CSDN 皮肤主题: 创作都市 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值