ROS与传感器教程-GPS坐标转换&Rviz显示轨迹

ROS与传感器教程-GPS坐标转换&Rviz显示轨迹

说明：

• 介绍如何根据获取的GPS坐标转换成rviz可显示的轨迹

思路：

• GPS信息是无法直接绘制轨迹的，因为其x，y为经纬度，z为高度，单位不一样
• 将GPS轨迹，从经纬度WGS-84坐标转换到真实世界xyz坐标系下
• 计算出每个gps坐标相对与第一个坐标的位置（m为单位），然后累加得到轨迹
• 在ROS框架在，读取GPS信息，并发布真实坐标系下的坐标话题（用Rviz可显示）

步骤：

• 创建gps_rviz包

cd ~/catkin_ws/src
catkin_create_pkg gps_rviz std_msgs rospy roscpp

• 创建gps.cpp

mkdir -p ~/catkin_ws/src/gps_rviz/src
cd ~/catkin_ws/src/gps_rviz/src
touch src/gps.cpp
vim src/gps.cpp

• 内容如下：

#include <ros/ros.h>
#include "turtlesim/Pose.h"
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <math.h>

struct my_pose
{
    double latitude;
    double longitude;
    double altitude;
};
//角度制转弧度制
double rad(double d) 
{
    return d * 3.1415926 / 180.0;
}
//全局变量
static double EARTH_RADIUS = 6378.137;//地球半径
ros::Publisher state_pub_;
nav_msgs::Path ros_path_;
bool init;
my_pose init_pose;


void gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg_ptr)
{
    //初始化
    if(!init)
    {
        init_pose.latitude = gps_msg_ptr->latitude;
        init_pose.longitude = gps_msg_ptr->longitude;
        init_pose.altitude = gps_msg_ptr->altitude;
        init = true;
    }
    else
    {
    //计算相对位置
        double radLat1 ,radLat2, radLong1,radLong2,delta_lat,delta_long;
        radLat1 = rad(init_pose.latitude);
        radLong1 = rad(init_pose.longitude);
        radLat2 = rad(gps_msg_ptr->latitude);
        radLong2 = rad(gps_msg_ptr->longitude);
        //计算x
        delta_lat = radLat2 - radLat1;
        delta_long = 0;
        double x = 2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat1 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ));
        x = x*EARTH_RADIUS*1000;

        //计算y
        delta_lat = 0;
        delta_long = radLong1  - radLong2;
        double y = 2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat2 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ) );
        y = y*EARTH_RADIUS*1000;

        //计算z
        double z = gps_msg_ptr->altitude - init_pose.altitude;

        //发布轨迹
        ros_path_.header.frame_id = "path";
        ros_path_.header.stamp = ros::Time::now();  

        geometry_msgs::PoseStamped pose;
        pose.header = ros_path_.header;

        pose.pose.position.x = x;
        pose.pose.position.y = y;
        pose.pose.position.z = z;

        ros_path_.poses.push_back(pose);

        ROS_INFO("( x:%0.6f ,y:%0.6f ,z:%0.6f)",x ,y ,z );

        state_pub_.publish(ros_path_);
    }
}
int main(int argc,char **argv)
{
    init = false;
    ros::init(argc,argv,"gps_subscriber");
    ros::NodeHandle n;
    ros::Subscriber pose_sub=n.subscribe("/gps/fix",10,gpsCallback);
        
    state_pub_ = n.advertise<nav_msgs::Path>("gps_path", 10);



    ros::spin();
    return 0;
}

• 编辑CMakeLists.txt

• 在之前使用catkin_create_pkg创建包gps_rviz ，会得到 package.xml 和 CMakeLists.txt两个文件

• 增加如下代码到CMakeLists.txt：

include_directories(include ${catkin_INCLUDE_DIRS})

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

• 编译

cd ~/catkin_ws
catkin_make  

运行节点

• 新开终端执行：

roscore

• 新开终端执行gps：

source ~/catkin_ws/devel/setup.bash
rosrun gps_rviz gps

• 新开终端执行rviz：

source ~/catkin_ws/devel/setup.bash
rviz

• 订阅话题gps_path， 移动gps，即可在rviz上看到对应的轨迹