RVIZ TF/Axel/path/odometry/PointCloud多种话题可视化操作

TF

TF 功能包可以让用户随时间跟踪多个坐标系

• 查看指定两个坐标系的变换关系
  tf_monitor <source_frame> <target_frame>
  tf_echo <source_frame> <target_frame>
• 发布两个坐标系之间的静态关系(两个坐标系不发生相对位置的变化)
  static_transform_publisher
  x y z yaw pitch roll (qx qy qz qw) frame_id child_frame_id period_in_ms
  rosrun tf view_frame 生成PDF版的整个TF树

静态TF坐标变换
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 1 map odom 20" />

Axes

rosrun tf view_frames 可以打印系统中当前frame的连接关系。
tf_echo 查看两个坐标系的静态关系

path 轨迹可视化

就是将追踪对象的位置随着时间的变化给可视化出来
主要用 geometry_msgs/PoseStamped数组, 里面包含多个pose点
poses[i].stamp poses[i].pose.position

话题类型：nav_msgs/Path
消息类型：

std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/PoseStamped[] poses
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w

Odometry

刚体位姿的可视化 下一帧相对于当前帧的旋转和平移

话题类型：nav_msgs/Odometry
消息类型：pose & twist

std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[36] covariance
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance

PointCloud

sensor_msgs::PointCloud数据结构

sensor_msgs::PointCloud
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Point32[] points 一帧点云所包含的所有点
float32 x
float32 y
float32 z
sensor_msgs/ChannelFloat32[] channels
string name
float32[] values
 
channels[] 数组中所包含的信息
channels[0].values[i] point_id
channels[1].values[i] camera_id
channels[2].values[i] p_u 点所对应的像素坐标
channels[3].values[i] p_v
channels[4].values[i] velocity_x 点所对应的速度
channels[5].values[i] velocity_y
channels[6].values[i] gx 点所对应的重力加速度
channels[7].values[i] gy
channels[8].values[i] gz

sensor_msgs::PointCloud使用示例

#include <map>
#include <Eigen/Core>
#include <Eigen/Geometry>

//订阅特征点
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
    //读取pointCloud message
    for (unsigned int i = 0; i < feature_msg->points.size(); i++)
    {
        int feature_id = feature_msg->channels[0].values[i];
        int camera_id = feature_msg->channels[1].values[i];
        double x = feature_msg->points[i].x;
        double y = feature_msg->points[i].y;
        double z = feature_msg->points[i].z;
        double p_u = feature_msg->channels[2].values[i];
        double p_v = feature_msg->channels[3].values[i];
        double velocity_x = feature_msg->channels[4].values[i];
        double velocity_y = feature_msg->channels[5].values[i];
        if(feature_msg->channels.size() > 5)
        {
            double gx = feature_msg->channels[6].values[i];
            double gy = feature_msg->channels[7].values[i];
            double gz = feature_msg->channels[8].values[i];
            pts_gt[feature_id] = Eigen::Vector3d(gx, gy, gz);
            //printf("receive pts gt %d %f %f %f\n", feature_id, gx, gy, gz);
        }
        ROS_ASSERT(z == 1);
        Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
        xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
        //压入 map vector
        featureFrame[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
    }
    double t = feature_msg->header.stamp.toSec();
    estimator.inputFeature(t, featureFrame);
    return;
}