周口住房和城乡建设网站,官网优化公司,北京网站建设公司报价,做网站有哪些程序文章目录 下载数据集创建功能包主要代码使用 下载数据集
链接: https://pan.baidu.com/s/1zSXyNhMNQdaFlDFziDse1Q 密码: 6j6u
其中话题/gps/fix和/imu_correct分别为sensor_msgs/NavSatFix类型的gps数据和sensor_msgs/Imu类型的imu数据
查看相关话题信息
创建功能包
cat… 文章目录 下载数据集创建功能包主要代码使用 下载数据集
链接: https://pan.baidu.com/s/1zSXyNhMNQdaFlDFziDse1Q 密码: 6j6u
其中话题/gps/fix和/imu_correct分别为sensor_msgs/NavSatFix类型的gps数据和sensor_msgs/Imu类型的imu数据
查看相关话题信息
创建功能包
catkin_create_pkg display_trajectory roscpp nav_msgs sensor_msgs turtlesim主要代码
GPS坐标是经纬度无法直接在rviz中形成轨迹本程序实现了以下功能 将GPS轨迹从经纬度WGS-84坐标转换到真实世界xyz坐标系东北天ENU下思路计算出每个gps坐标相对与第一个坐标的距离m为单位比较相邻两点的经纬度变化赋予位移的方向然后累加得到轨迹
#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.hstruct 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,x,y;radLat1 rad(init_pose.latitude);radLong1 rad(init_pose.longitude);radLat2 rad(gps_msg_ptr-latitude);radLong2 rad(gps_msg_ptr-longitude);//计算xdelta_long 0;delta_lat radLat2 - radLat1; //(radLat1,radLong1)-(radLat2,radLong1)if(delta_lat0)x 2*asin( sqrt( pow( sin( delta_lat/2 ),2) cos( radLat1 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ));elsex-2*asin( sqrt( pow( sin( delta_lat/2 ),2) cos( radLat1 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ));x x*EARTH_RADIUS*1000;//计算ydelta_lat 0;delta_long radLong2 - radLong1; //(radLat1,radLong1)-(radLat1,radLong2)if(delta_long0)y 2*asin( sqrt( pow( sin( delta_lat/2 ),2) cos( radLat2 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ) );elsey-2*asin( sqrt( pow( sin( delta_lat/2 ),2) cos( radLat2 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ) );//double y 2*asin( sin( delta_lat/2 ) cos( radLat2 )*cos( radLat2)* sin( delta_long/2 ) );y y*EARTH_RADIUS*1000;//计算zdouble z gps_msg_ptr-altitude - init_pose.altitude;//发布轨迹ros_path_.header.frame_id odom;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_to_rviz);ros::NodeHandle n;ros::Subscriber pose_subn.subscribe(/gps/fix,10,gpsCallback);state_pub_ n.advertisenav_msgs::Path(gps_path, 10);ros::spin();return 0;
}使用
cd ${ws_root}
source ./devel/setup.bash
rosrun display_trajectory display_trajectory_node
