程序员经验分享-hwhale

Rviz显示理想的运动的轨迹，并对比实时的运动轨迹

2023-05-16

20210505更新

前天的bug，修改了marker点的尺寸和颜色后在rviz中不显示的问题解决了，代码什么的完全没有改，只是用了sudo apt-get upgrade，把ros的一些包和库都升级了，然后发现这个问题就解决了，可以正常显示了，看来这个upgrade还是很用的，以前比较少用这个，觉得没什么大作用，没想到，今天编译其他东西后，发现连带着以前的bug也解决了
marker

2021.05.05

20210503更新

显示多个Marker点

Target：想看看机械臂需要停驻的点
Method：

用Rviz里面的marker和markerarray显示多个点：
先上代码：

#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>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <fstream>
#include <iostream>
#include <sstream>
using namespace std;

double PI = 3.1415926;

main (int argc, char **argv)
{
    // show the ground ture path
    ros::init (argc, argv, "showpath");

    ros::NodeHandle ph;
    ros::Publisher vis_pub = ph.advertise<visualization_msgs::MarkerArray>( "visualization_marker", 10 );

    ifstream selected_robot("/home/will/catkin_ws/src/gazebo_mobile_manipulator/data/uvc_point.txt");
	ifstream selected_robot_euler_angle("/home/will/catkin_ws/src/gazebo_mobile_manipulator/data/uvc_angle.txt");

    if (!selected_robot.is_open())
	{
		cout << "can not open selected_robot file" << endl;
		return 0;
	}

    if (!selected_robot_euler_angle.is_open())
	{
		cout << "can not open selected_robot_euler_angle file" << endl;
		return 0;
	}

    char c, c2;
    int line=0;
    int line2=0;
    while(selected_robot.get(c))
    {
        if (c=='\n')
            line++;
    }
    while(selected_robot_euler_angle.get(c2))
    {
        if (c2=='\n')
            line2++;
    }
    printf("line sum: %d %d\n", line, line2);
	double position[line+1][3];

    double euler[line2+1][3];

    selected_robot.clear();
    selected_robot.seekg(ios::beg);    
    selected_robot_euler_angle.clear();
    selected_robot_euler_angle.seekg(ios::beg);
    //从data1文件中读入int数据
	for (int i = 0; i < line+1; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			selected_robot >> position[i][j];
			selected_robot_euler_angle >> euler[i][j];
		}
	}
    ros::Rate loop_rate(50);
    int count = 0;
    while (ros::ok())
    {
        // Marker
        visualization_msgs::MarkerArray marker;
        for (int i = 0; i < line+1; i++)
        {
            visualization_msgs::Marker points; 
            points.header.frame_id = "base";
            points.header.stamp = ros::Time::now();
            points.ns = "points";
            points.action = visualization_msgs::Marker::ADD;
            points.id = i;
            points.type = visualization_msgs::Marker::SPHERE_LIST;
            points.scale.x = 0.1;
            points.scale.y = 0.1;
            points.scale.z = 0.1;

            points.pose.orientation.x = 0;
            points.pose.orientation.y = 0;
            points.pose.orientation.z = 0;
            points.pose.orientation.w = 1;


            points.color.r = 1.0;
            points.color.a = 1.0;

            geometry_msgs::Point p;
            p.x = position[i][0];
            p.y = position[i][1];
            p.z = position[i][2];
            cout << i << " x y z= " << position[i][0] << "   " <<  position[i][1] << "  " << position[i][2] << endl;
            cout << endl << endl;
    
            points.points.push_back(p);
            marker.markers.push_back(points); 
        }
        cout << "MarkerArray points size: " << marker.markers.size() << endl;
        vis_pub.publish(marker);
        ros::spinOnce();               // check for incoming messages

        loop_rate.sleep();
    }

    return 0;
}

效果如下：

maker
产生了对应的点，但是很奇怪，非常不明显，我设置点的尺寸大小和颜色好像没有作用一样，我换了好几种尺寸和颜色，最后编译完，显示的还是原来的模样，弄了一上午，也没找到解决方法，如果有路过的大神，望指点一二。

虽然不是很明显，但是能看到对应的点，也算可以讲究着用，其实我也试了marker那个显示工具，也是一样的效果，太奇怪了，懵逼。
2021.05.03

Target：

显示两条轨迹，一个是Ground true trajectory，另一个是机器人仿真的End-effector trajectory

Method：

1. 建立Ground Ture 节点

1）写一个节点发布Ground true的空间坐标和姿态，建一个以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>

#include <fstream>
#include <iostream>
#include <sstream>
using namespace std;

double PI = 3.1415926;
//Subscribe information about EE
double position[16][3] = {0};
double euler[16][3] = {0};

main (int argc, char **argv)
{
    // show the ground ture path
    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="base";

	ifstream selected_robot("/home/will/catkin_ws/src/gazebo_mobile_manipulator/src/py_script/uvc_point.txt");
	ifstream selected_robot_euler_angle("/home/will/catkin_ws/src/gazebo_mobile_manipulator/data/uvc_angle.txt");

    if (!selected_robot.is_open())
	{
		cout << "can not open selected_robot file" << endl;
		return 0;
	}

    if (!selected_robot_euler_angle.is_open())
	{
		cout << "can not open selected_robot file" << endl;
		return 0;
	}
    //从data1文件中读入int数据
	for (int i = 0; i < 16; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			selected_robot >> position[i][j];
			selected_robot_euler_angle >> euler[i][j];
		}
	}
    
	for (int i = 0; i < 16; i++)
	{
		for (int j = 0; j < 3; j++)
		{
            cout << euler[i][j] << ", ";
		}
        cout << endl;
	}

    //ros::Rate loop_rate(1);
    int count = 0;
    while (ros::ok())
    {

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

        geometry_msgs::PoseStamped this_pose_stamped;
        this_pose_stamped.pose.position.x = position[count][0];
        this_pose_stamped.pose.position.y = position[count][1];
        this_pose_stamped.pose.position.z = position[count][2];


        geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromRollPitchYaw(euler[count][0]/180*PI, euler[count][1]/180*PI, euler[count][2]/180*PI);
        cout << "goal_quat.x = "<< goal_quat.x <<endl;
        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="base";
        path.poses.push_back(this_pose_stamped);

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

        last_time = current_time;
        count ++;
        if (count > 15)
        {
            count =15;
        }
        /*
        if (count == 16)
        {
            ros::shutdown();
        }
        */
        //loop_rate.sleep();
    }

    return 0;
}

从代码中可以发现，我是从两个txt文件中读取位置和姿态的信息，然后建立一个topic，以trajectory命名，把这两个信息发布出去。

2）在Rviz中读取这个topic，显示对应的轨迹
打开Rviz后，点击左下角的Add，然后添加Path，如下图所示：
add path编译（别忘了在CMakeLists中添加生成执行文件那两行代码）完成后，先运行showpath.cpp
然后在path中的Topic选择刚刚我们发布的/trajectory话题，就可以看到Ground true的轨迹了，如下图所示：
最终效果

2. 仿真模型实时发布末端轨迹

这一步就要比上一步发布Ground true trajectory要麻烦一点。

1）调用TF监听当前模型的End-effector的位姿
这一步其实也不用单独建一个node来做这件事，只需要在我们控制robot运动的node里面多发一个topic，这个topic发送的信息源于TF的监听信息（position and orientation），根据实际情况，自由安排这个topic放在哪里。例程如下，有些内容有删减掉了，监听部分核心内容都在里面了。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Path.h>

using namespace Eigen;
using namespace std;

double PI = 3.1415926;

int main(int argc, char  *argv[])
{
    
    ros::init(argc, argv, "pub_command");
    ros::NodeHandle nh;
    // tf坐标发布器
    ros::Publisher pub = nh.advertise<sensor_msgs::JointState>("/joint_commands", 20);
    ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("EE_trajectory",1, true);

    ros::Rate r(20);
    tf::TransformBroadcaster broadcaster;
    tf::TransformListener listener;

    //Publish EE trajectory
    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

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

    while(nh.ok()){

        // listen TF publishing the message about /base /EE
        tf::StampedTransform transform;
		try
		{
			listener.waitForTransform("/base", "/EE", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/base", "/EE", ros::Time(0), transform);
		}
		catch (tf::TransformException &ex) 
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
        ROS_INFO("Transform x y z: %f %f %f",transform.getOrigin().x(),transform.getOrigin().y(),transform.getOrigin().z());
        ROS_INFO("Transform Rotation x y z w: %f %f %f %f",transform.getRotation().x(),transform.getRotation().y(),
        transform.getRotation().z(),transform.getRotation().w());

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

        geometry_msgs::PoseStamped this_pose_stamped;
        this_pose_stamped.pose.position.x = transform.getOrigin().x();
        this_pose_stamped.pose.position.y = transform.getOrigin().y();
        this_pose_stamped.pose.position.z = transform.getOrigin().z();

        this_pose_stamped.pose.orientation.x = transform.getRotation().x();
        this_pose_stamped.pose.orientation.y = transform.getRotation().y();
        this_pose_stamped.pose.orientation.z = transform.getRotation().z();
        this_pose_stamped.pose.orientation.w = transform.getRotation().w();

        this_pose_stamped.header.stamp=current_time;
        this_pose_stamped.header.frame_id="base";
        path.poses.push_back(this_pose_stamped);
        path_pub.publish(path);
        
        ros::spinOnce();
        r.sleep();
    }
}

注意：
1.要添加#include <tf/transform_listener.h>这个头文件，TF监听需要用到的函数都在里面了。
2. listener.lookupTransform("/base", "/UV", ros::Time(0), transform);代码中的这一句，读取的是两个坐标系之间的空间位置和旋转关系，从基座标系/base到末端的坐标系/EE，对应的名字，查看自己的URDF文件，或者直接在Rviz里面也可以看到
3. ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("EE_trajectory",1, true);发布的topic名为EE_trajectory
4.transform.getOrigin().x();用这个函数就可以读取我们指定的两个坐标系之间的位置关系，用transform.getRotation()这个函数可以读取我们指定的两个坐标系之间的旋转关系，注意，读取的是四元数，应该只有四元数，我其实想读取RPY的，但是查了手册，没有读取RPY的函数，所以只能用四元数了，要是有新的TF库更新，欢迎大家告诉我，让我也update一下。不知道tf2有没有这个功能，我一直在用tf，没查看过tf2.

2）这一步也跟上一个创建Ground true节点的操作是一样的
在Rviz中Add一个新的Path，然后在topic中选择EE_trajectory这个topic，接着，控制机器人运动，就可以看到实时生成的轨迹了，下过如下图：
showtrajectory上图中红色线是实时生成的轨迹，绿色线就是ground true的轨迹了，这样一对比，就很清楚的知道控制部分的代码效果如何了。

此外，如果觉得线条不够粗，可以在path中选择线性，默认的line style是lines，但是还有另一种Billboards，这个类型多了一个参数，就是line width，然后就可以根据实际需要，改变线宽了，但是我用这个类型的时候，不知道为什么，rviz会突然自己关闭，很奇怪，用lines类型就没出现这种情况。
line_width

Reference

  1. ROS中rviz显示运动轨迹的常见方法:
    https://blog.csdn.net/u013834525/article/details/80447931?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7Edefault-5.control&dist_request_id=&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7Edefault-5.control

  2. ROS之rviz显示历史运动轨迹、路径的各种方法（visualization_msgs/Marker、nav_msgs/Path）:
    https://blog.csdn.net/u013834525/article/details/80447931?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7Edefault-5.control&dist_request_id=&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7Edefault-5.control

  3. 详解ROS中的TF使用：
    https://blog.csdn.net/qq_37394634/article/details/105494587?utm_medium=distribute.pc_aggpage_search_result.none-task-blog-2aggregatepagefirst_rank_v2~rank_aggregation-1-105494587.pc_agg_rank_aggregation&utm_term=ros+%E4%B8%ADtf%E7%9A%84%E4%BD%BF%E7%94%A8&spm=1000.2123.3001.4430

  4. tf::Transform Class Reference:
    http://docs.ros.org/en/jade/api/tf/html/c++/classtf_1_1Transform.html

  5. ROS之rviz显示历史运动轨迹、路径的各种方法（visualization_msgs/Marker、nav_msgs/Path）：
    https://haoqchen.site/2018/05/25/ROS-show-trajectory/

  6. ROS在rviz中实时显示轨迹和点：
    https://yunchengjiang.blog.csdn.net/article/details/108514336?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-3.control&dist_request_id=1332036.7678.16191425010885527&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-3.control

  7. rviz中使用MarkerArray绘制地图线：https://blog.csdn.net/qq_35277038/article/details/84848555

  8. rviz中的标记（markers）和交互标记（interactive markers）：https://blog.csdn.net/wxflamy/article/details/79347689?utm_medium=distribute.pc_relevant.none-task-blog-baidujs_title-0&spm=1001.2101.3001.4242

  9. Rviz教程-Marker：点和线（C++）：https://blog.csdn.net/wilylcyu/article/details/56847503?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-3.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-3.control

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

Rviz显示理想的运动的轨迹，并对比实时的运动轨迹 的相关文章

  • Python小技巧之——巧用with语句实现异常处理

    Python的异常处理语句try except大家都很熟悉了 xff0c 例如 xff1a try 1 0 except Exception as ex print ex integer division or modulo by zero

  • 将lwip1.4.1工程移植至lwip2.1.2记录

    将lwip1 4 1工程移植至lwip2 1 2记录 1 ip addr结构体2 etharp h3 cc h与arch h4 tcp impl h 关于二者文件 功能等差异网上已有很多文章介绍 xff0c 类似这个文档有简单说了下这两个版

  • 在Windows和Ubuntu上安装VNC连接远程服务器

    如果你是公用的服务器的管理员需要添加一些用户 xff0c 并配置VNC服务以便远程访问 xff0c 简要介绍一下配置方法 具体的命令可以参照命令手册去查看 man span class hljs command span class hlj

  • 变频器的工作原理及其电路分析

    变频器简单的说就是结合了变频技术和微电子技术研制出来的可以改变输入电源的频率得到另外一种频率电源输出的设备 其输入的电源就是我们工业上面使用的电源 xff0c 一般都是电压和频率都固定不变的交流电 240v或者380v交流电 通过内置的一些

  • 欠拟合、过拟合及其解决方法

    在我们机器学习或者训练深度神经网络的时候经常会出现欠拟合和过拟合这两个问题 xff0c 但是 xff0c 一开始我们的模型往往是欠拟合的 xff0c 也正是因为如此才有了优化的空间 xff0c 我们需要不断的调整算法来使得模型的表达能拿更强

  • ubuntu18.04安装ROS Melodic的详细过程以及填坑经历

    一 版本说明 ROS官方将在2021年不再维护Kinetic xff0c 后续使用Ubuntu18 04 43 Melodic组合 xff0c Melodic支持时间到2023年5月 二 安装前Ubuntu18 04设置 打开Ubuntu1

  • win10和ubuntu20双系统设置默认启动系统为win10

    在win10下安装了Ubuntu20 04系统 xff0c 默认情况下 xff0c 启动的是Ubuntu系统 要将默认启动系统设置成win10 xff0c 方法如下 xff1a 1 进入ubuntu系统 xff0c 按住Ctrl 43 Al

  • Keil添加芯片支持包（Pack）

    1 前言 一直用STM32的芯片 xff0c 现在想看看工程是否可以在其他厂家的芯片上跑 xff0c 可是keil的Device中只有ST厂家的 因此 xff0c 尝试在keil中添加其他厂家的芯片支持包 2 keil软件内安装 点击工具栏

  • Qt 设置窗体大小和背景颜色

    1 一种方法是设置它的最大窗口值和最小窗口值 xff0c 并且使最大值和最小值相等 简单的示例 xff1a setMinimumSize 370 150 setMaximumSize 370 150 此时窗口大小便被固定为 xff08 37

  • Shell 脚本详解

    简介 shell xff1a 蛋 壳 shell脚本是在操作系统外 xff0c 可以直接调用系统内核命令的一个脚本语言 shell脚本可以分为两大类组成 xff1a 1 命令行 xff08 系统命令行 xff09 2 脚本语法 xff08

  • Windows——电脑不能连接手机热点(WLAN显示已经禁用)的解决办法

    笔记本电脑提示 xff1a 已关闭无线功能 基于这篇博客之上 xff0c 在第二步中 xff0c 关闭WLAN AutoConfig 服务 xff0c 之后重新打开WLAN AutoConfig 服务 xff0c 即可

  • Ubuntu——系统语言由英文切换到中文的方法

    一 方法一 ubuntu设置系统语言为中文 二 方法二 若方法一中不能拖动中文输入法到第一行 xff0c 则可以直接采取卸载英文输入法 xff0c 这样就中文输入法到第一行了 xff0c 切换成中文了 英文输入法可以根据需要考虑是否安装 一

  • RealSense D435——基本介绍

    一 结构介绍 采用的是结构光Tof成像方案 正面的四个摄像头从左至右 xff0c 依次是左红外相机 红外点阵投影仪 右红外相机 RGB相机 xff08 前三个负责形成深度图 xff0c 最后一个就形成RGB图 xff09 二 小贴士 RGB

  • RealSense D435——相机内参获取

    RealSense D435 相机内参获取 一 参考博客二 小贴士2 1 遇到的问题及解决方案问题一描述问题一解决方法问题二描述问题二解决方法 一 参考博客 RealSense D435内参获取环境配置 xff1a Realsense D4

  • Vscode——报错解决：Unable to start debugging.Unexpected GDB output from command. 或 程序点击运行一直无结果

    一 报错截图 1 Unable to start debugging Unexpected GDB output from command 2 程序点击运行一直无结果 二 原因 路径中含有中文 三 解决办法 将文件放入不包含中文的路径下

  • Github——合并分支

    一 当两个分支不一样时 xff0c 会出现下面的标志 xff08 前提是设定了分支保护 xff09 xff0c 点击Compare amp pull request 二 选择双方分支 三 处理请求 四 确认请求

  • 基于四旋翼飞行器的陀螺仪、加速度计、磁力计传感器说明

    一 什么是磁力计 加速度计和陀螺仪以及他们之间的区别 1 什么是陀螺仪 加速度计和磁力计 xff1f xff08 1 xff09 陀螺仪 xff08 Gyroscope GYRO Sensor xff09 也叫地感器 xff0c 三轴陀螺仪

  • 操作系统（二） -- 操作系统的接口与实现

    前言操作系统的接口 什么是操作系统的接口POSIX标准 系统调用的实现 1 xff0c 用户程序能不能直接调用系统内核2 xff0c 如果不能直接调用 xff0c 为什么 xff1f 如何实现的3 xff0c 用户程序如何才能调用系统内核系

  • 自动驾驶路径规划技术-高速公路路径规划

    Path Planning Highway Driving project Github https github com williamhyin CarND Path Planning Email williamhyin 64 outlo

  • FYI, MySQL高效分页

    在Percona Performance Conference 2009大会上来自yahoo的Surat Singh Bhati surat 64 yahoo inc com 和 Rick James rjames 64 yahoo inc

随机推荐

热门标签