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

20210505更新

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

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 pathros::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()){// Markervisualization_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 messagesloop_rate.sleep();}return 0;
}

效果如下：

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

虽然不是很明显，但是能看到对应的点，也算可以讲究着用，其实我也试了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 pathros::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 messageslast_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，如下图所示：
编译（别忘了在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 trajectoryros::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 /EEtf::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，接着，控制机器人运动，就可以看到实时生成的轨迹了，下过如下图：
上图中红色线是实时生成的轨迹，绿色线就是ground true的轨迹了，这样一对比，就很清楚的知道控制部分的代码效果如何了。

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

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