在机器人建图过程中,需要激光传感器和机器人里程信息,里程信息指的是机器人相对于某一个点的距离。一般来说,本车坐标系“base_link"原点相对于odom坐标系原点的距离。在导航功能包集中消息类型为nav_msgs/Odometry。
通过rosmsg show nav_msgs/Odometry
可以看到:
std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Pose 消息提供了机器人的位姿信息
geometry_msgs/Twist 消息提供了机器人的速度信息
位姿信息中包含两个结构:一个是显示了欧式坐标系中的位置,另一个使用四元数显示了机器人的方向。
速度信息中包含两个结构:一个是线速度,一个是角速度
ps:一般情况下,我们经常使用的是线速度x和角速度z,使用线速度x是为了知道机器人向前移动或向后移动,使用角速度z是为了知道机器人向左/右旋转。
如果你不想接入采图车的轮速编码器,不想去推算运动学模型,可以借助车上搭载的差分GPS,通过其输出的经纬高、航向信息转换为里程计信息的输出。
“/nmea_sentence"为差分GPS发布的的topic,包含的是一系列逗号分开的字符串。
可以分为三步走:
- 从字符串中提取出经纬高,航向信息
- 转换成机器人相应的位置、航向信息。
- 通过nav_msgs/odometry 发布出去。
/**********************
* @FileName: gps_odometry.cpp
* @Author: LZY
* @DateTime: 2019-02-22
* @Description: GPS to odom
**********************/
#include<iostream>
#include<ros/ros.h>
#include<geometry_msgs/QuaternionStamped.h>
#include<nmea_msgs/Sentence.h>
#include<nav_msgs/Odometry.h>
#include<tf/transform_broadcaster.h>
#include<fstream>
#include<std_msgs/String.h>
#include<string.h>
#include<cmath>
#include "geodesy/utm.h"
#include <unistd.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include "gps_info.h"
#include "WGS84_UTM.h"
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
ros::Publisher g_odom_pub;
double origin_x=658357.0000,origin_y=3292682.0000; //yanjiuzongyuan
//double origin_x = 670460.0000, origin_y = 3281318.0000;// yuzui
static int GetComma(int num,const char *str)
{
int i,j=0;
for(i=0;i<strlen(str);i++)
{
if(str[i]==',')
j++;
if(j==num)
return i+1;//返回当前找到的逗号位置的下一个位置
}
return 0;
}
static double get_double_number(const char *s)
{
char buf[128];
int i;
double rev;
i=GetComma(1,s); //得到数据长度
strncpy(buf,s,i);
buf[i]=0; //加字符串结束标志
rev=atof(buf); //字符串转float
return rev;
}
/***DegToRad***/
double DegToRad(double deg)
{
return (deg* pi / 180.0 );
}
/***RadToDeg***/
double RadToDeg(double rad)
{
return (rad*180.0/pi);
}
void gpsCallback(const nmea_msgs::Sentence::ConstPtr& msg )
{
static tf::TransformBroadcaster gps_tf;
GPS_INFO rmc_info;
GPS_INFO *prmc_info;
prmc_info=&rmc_info;
int i,tmp;
float lati_cent_tmp, lati_second_tmp;
float long_cent_tmp, long_second_tmp;
char c,b;
const char *buf=msg->sentence.c_str();//buf指向字符串首地址
c=buf[5];
b=buf[2];
if(b=='P'&&c=='C')//解析GPRMC字段
{
// ROS_INFO("***************************************************************");
// ROS_INFO("The GPRMC string is:%s",buf);
prmc_info->D.hour= (buf[7]-'0')*10+(buf[8]-'0');
prmc_info->D.minute=(buf[9]-'0')*10+(buf[10]-'0');
prmc_info->D.second=(buf[11]-'0')*10+(buf[12]-'0');
tmp=GetComma(9,buf);//寻找第9个逗号的下一个字符串的序号
//ROS_INFO("the number is:%d",tmp);
prmc_info->D.day= (buf[tmp+0]-'0')*10+(buf[tmp+1]-'0');
prmc_info->D.month= (buf[tmp+2]-'0')*10+(buf[tmp+3]-'0');
prmc_info->D.year= (buf[tmp+4]-'0')*10+(buf[tmp+5]-'0')+2000;
prmc_info->status= buf[GetComma(2,buf)]; //位置状态,A=有效,V=无效
//这里的经纬度 (DDDmm.mm),字符串转换为双精度数,然后转为度deg
prmc_info->latitude= get_double_number(&buf[GetComma(3,buf)]); //纬度,单位为度
prmc_info->NS= buf[GetComma(4,buf)];
prmc_info->longitude=get_double_number(&buf[GetComma(5,buf)]); //经度,单位为度
prmc_info->EW= buf[GetComma(6,buf)];
int lat_d=prmc_info->latitude/100;
int long_d = prmc_info->longitude/100;
double lat_fd = (prmc_info->latitude-lat_d*100)/60;
double long_fd = (prmc_info->longitude-long_d*100)/60;
prmc_info->latitude_d =lat_d+lat_fd;
prmc_info->longitude_d = long_d+long_fd;
prmc_info->direction=get_double_number(&buf[GetComma(8,buf)]);//航向角,一般转为弧度进行计算
Local_data local_XY;
geographic_msgs::GeoPoint gp;
gp.latitude = prmc_info->latitude_d;
gp.longitude =prmc_info->longitude_d;
geodesy::UTMPoint pt(gp);
local_XY.X = pt.easting;
local_XY.Y = pt.northing;
double yaw = DegToRad(-(prmc_info->direction-90));
//ROS_INFO("Local_X = %lf ", local_XY.X);
//ROS_INFO("Local_Y = %lf ", local_XY.Y);
tf::Quaternion qua;
qua.setRPY(0,0,yaw);
/***publish gps_odom**/
nav_msgs::Odometry odom;
odom.header.stamp=ros::Time::now();
odom.header.frame_id="map";
odom.child_frame_id="gps";
odom.pose.pose.position.x=local_XY.X-origin_x;
odom.pose.pose.position.y=local_XY.Y-origin_y;
odom.pose.pose.orientation.x=qua.x();
odom.pose.pose.orientation.y=qua.y();
odom.pose.pose.orientation.z=qua.z();
odom.pose.pose.orientation.w=qua.w();
g_odom_pub.publish(odom);
/***publish world to gps tf **/
tf::StampedTransform trans;
trans.stamp_ = odom.header.stamp;
trans.frame_id_ =odom.header.frame_id;
trans.child_frame_id_ = odom.child_frame_id;
trans.setOrigin(tf::Vector3(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z));
trans.setRotation(qua);
gps_tf.sendTransform(trans);
}
}
int main(int argc,char **argv)
{
ros::init(argc,argv,"odometry");
ros::NodeHandle nh;
ros::Subscriber sub=nh.subscribe("/nmea_sentence",1000, gpsCallback);
g_odom_pub=nh.advertise<nav_msgs::Odometry>("gps/odometry",1);
ros::spin();
return 1;
}
解算出GPS的经纬度以后,需要发布里程计信息,父坐标系为“map”,子坐标系为“gps”,子坐标系围绕着父坐标系运动,存在一定的运动轨迹。
1)发布里程计信息。将车辆偏航角(旋转)由四元数表示,
tf::Quaternion qua;
qua.setRPY(0,0,yaw);
odom.pose.pose.orientation.x=qua.x();
odom.pose.pose.orientation.y=qua.y();
odom.pose.pose.orientation.z=qua.z();
odom.pose.pose.orientation.w=qua.w();
odom.pose.pose.position.x=local_XY.X-origin_x;
odom.pose.pose.position.y=local_XY.Y-origin_y;
ros::Publisher g_odom_pub;
g_odom_pub.publish(odom);
即可发布里程计
2)发布GPS与world坐标系的tf关系,后续
tf::StampedTransform trans;
trans.stamp_ = odom.header.stamp;
trans.frame_id_ =odom.header.frame_id;
trans.child_frame_id_ = odom.child_frame_id;
trans.setOrigin(tf::Vector3(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z));
trans.setRotation(qua);
gps_tf.sendTransform(trans);