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

最新推荐文章于 2024-09-22 17:29:47 发布

Will_Ye

最新推荐文章于 2024-09-22 17:29:47 发布

阅读量5.9k

点赞数 7

分类专栏： ROS learning 文章标签： ROS

本文链接：https://blog.csdn.net/Will_Ye/article/details/116061302

版权

ROS learning 专栏收录该内容

42 篇文章 19 订阅

订阅专栏

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

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
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
详解ROS中的TF使用：
https://blog.csdn.net/qq_37394634/article/details/105494587?utm_medium=distribute.pc_aggpage_search_result.none-task-blog-2_{aggregatepage}first_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
tf::Transform Class Reference:
http://docs.ros.org/en/jade/api/tf/html/c++/classtf_1_1Transform.html
ROS之rviz显示历史运动轨迹、路径的各种方法（visualization_msgs/Marker、nav_msgs/Path）：
https://haoqchen.site/2018/05/25/ROS-show-trajectory/
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
rviz中使用MarkerArray绘制地图线：https://blog.csdn.net/qq_35277038/article/details/84848555
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
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