网站搜索优化排名,廊坊网站建设技术支持,wordpress支持页面模版,腾讯营销文章目录 概要解算过程获取初始化点经纬度坐标系转UTM计算航向角发布odom坐标 完整代码 概要
这篇博客主要介绍#xff0c;如何将GPS_fix、磁偏角转成odom信息。 PS:官方的驱动包中是自带odom信息#xff0c;但是对于原点的定义尚未找到出处#xff0c;故自己另外写了一套发… 文章目录 概要解算过程获取初始化点经纬度坐标系转UTM计算航向角发布odom坐标 完整代码 概要
这篇博客主要介绍如何将GPS_fix、磁偏角转成odom信息。 PS:官方的驱动包中是自带odom信息但是对于原点的定义尚未找到出处故自己另外写了一套发布odom信息。
解算过程
获取初始化点
第一个获取的GPS_fix点为初始点
initPose.latitude gpsFix-latitude;
initPose.longitude gpsFix-longitude;
initPose.altitude 0;
init true;经纬度坐标系转UTM
/*原点经纬度转UTM*/
geographic_msgs::GeoPoint gpInit;
gpInit.latitude initPose.latitude;
gpInit.longitude initPose.longitude;
geodesy::UTMPoint ptInit(gpInit);
initX ptInit.easting;
initY ptInit.northing;计算航向角
记得减去当地的磁偏角在这个网站进行查询。
tf::Quaternion qua;
tf::quaternionMsgToTF(odomMsg-pose.pose.orientation, qua);
double roll, pitch, yaw;//定义存储roll,pitch and yaw的容器
tf::Matrix3x3(qua).getRPY(roll, pitch, yaw); //进行转换
yaw yaw - 0.5 * M_PI MagDec / 180.0 * M_PI;发布odom坐标 /***publish gps_odom**/
nav_msgs::Odometry odom;
odom.header.stamp ros::Time::now();
odom.header.frame_id odom;
odom.pose.pose.position.x fixX - initX;
odom.pose.pose.position.y fixY - initY;
odom.pose.pose.orientation.xqua.x();
odom.pose.pose.orientation.yqua.y();
odom.pose.pose.orientation.zqua.z();
odom.pose.pose.orientation.wqua.w();
gpsOdomPub.publish(odom);完整代码
#include ros/ros.h
#include turtlesim/Pose.h
#include sensor_msgs/NavSatFix.h
#include geometry_msgs/PoseStamped.h
#include geometry_msgs/PolygonStamped.h
#include geometry_msgs/QuaternionStamped.h
#include geographic_msgs/GeoPoseStamped.h
#include geodesy/utm.h
#include nav_msgs/Odometry.h
#include nav_msgs/Path.h
#include math.h
#include message_filters/synchronizer.h
#include message_filters/subscriber.h
#include message_filters/sync_policies/approximate_time.h
#include message_filters/time_synchronizer.h
#include boost/thread/thread.hpp
#include iostream//全局变量
static double EARTH_RADIUS 6378.137;//地球半径class OdomPublisher
{public:OdomPublisher();void gpsCallback(const sensor_msgs::NavSatFixConstPtr gpsFix,const nav_msgs::Odometry::ConstPtr odomMsg);double rad(double d);private:ros::Publisher state_pub_, smallCarPub, gpsOdomPub;geometry_msgs::PolygonStamped carPolygon;nav_msgs::Path ros_path_;ros::NodeHandle n, nhPrivate;message_filters::Subscribersensor_msgs::NavSatFix *subGPS;message_filters::Subscribernav_msgs::Odometry *subOdom;typedef message_filters::sync_policies::ApproximateTimesensor_msgs::NavSatFix, nav_msgs::Odometry syncPolicy;message_filters::SynchronizersyncPolicy *sync;bool init;struct my_pose{double latitude;double longitude;double altitude;};my_pose initPose, fixPose;double initX, initY, MagDec;std::string gpsFixTopic, gpsOdomTopic, gpsOdomPubTopic;
};OdomPublisher::OdomPublisher():nhPrivate(~)
{ ROS_INFO(Initialization);nhPrivate.param(gpsFixTopic, gpsFixTopic, std::string(/gps/fix));nhPrivate.param(gpsOdomTopic, gpsOdomTopic, std::string(/odom));nhPrivate.param(gpsOdomPubTopic, gpsOdomPubTopic, std::string(/gps_fix/odom));nhPrivate.getParam(MagDec, MagDec);subGPS new message_filters::Subscribersensor_msgs::NavSatFix (n, gpsFixTopic.c_str(), 1);subOdom new message_filters::Subscribernav_msgs::Odometry (n, gpsOdomTopic.c_str(), 1);sync new message_filters::SynchronizersyncPolicy (syncPolicy(10), *subGPS, *subOdom);sync-registerCallback(boost::bind(OdomPublisher::gpsCallback, this, _1, _2));state_pub_ n.advertisenav_msgs::Path(/trajectory/gps, 10);smallCarPub n.advertisegeometry_msgs::PolygonStamped(/trajectory/car, 10);gpsOdomPub n.advertisenav_msgs::Odometry(gpsOdomPubTopic.c_str(),10); init false;
}//角度制转弧度制
double OdomPublisher::rad(double d)
{return d * 3.1415926 / 180.0;
}void OdomPublisher::gpsCallback(const sensor_msgs::NavSatFixConstPtr gpsFix,const nav_msgs::Odometry::ConstPtr odomMsg)
{// ROS_INFO_STREAM(Starting to work!!!);// std::cout Starting to work!!! std::endl;//初始化if(!init){initPose.latitude gpsFix-latitude;initPose.longitude gpsFix-longitude;initPose.altitude 0;init true;/*原点经纬度转UTM*/geographic_msgs::GeoPoint gpInit;gpInit.latitude initPose.latitude;gpInit.longitude initPose.longitude;geodesy::UTMPoint ptInit(gpInit);initX ptInit.easting;initY ptInit.northing;}else{geographic_msgs::GeoPoint gp;gp.latitude gpsFix-latitude;gp.longitude gpsFix-longitude;geodesy::UTMPoint pt(gp);double fixX pt.easting;double fixY pt.northing;tf::Quaternion qua;tf::quaternionMsgToTF(odomMsg-pose.pose.orientation, qua);double roll, pitch, yaw;//定义存储roll,pitch and yaw的容器tf::Matrix3x3(qua).getRPY(roll, pitch, yaw); //进行转换yaw yaw - 0.5 * M_PI MagDec / 180.0 * M_PI;qua.setRPY(0,0,yaw);// ROS_INFO(After optmized, yaw is %f, yaw);/***publish gps_odom**/nav_msgs::Odometry odom;odom.header.stamp ros::Time::now();odom.header.frame_id odom;odom.pose.pose.position.x fixX - initX;odom.pose.pose.position.y fixY - initY;odom.pose.pose.orientation.xqua.x();odom.pose.pose.orientation.yqua.y();odom.pose.pose.orientation.zqua.z();odom.pose.pose.orientation.wqua.w(); gpsOdomPub.publish(odom);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 odom.pose.pose.position;ros_path_.poses.push_back(pose);state_pub_.publish(ros_path_);}
}int main(int argc,char **argv)
{ros::init(argc,argv,gps_fix);OdomPublisher op;ros::spin();return 0;
}
