所用设备的输出:
[21:39:29:911] $GPTRA,043930.40,265.39,-00.05,000.00,4,20,0.00,0000*52␍␊
[21:39:29:916] $GPGGA,043930.40,2309.5803027,N,11320.5363700,E,1,20,0.7,40.0725,M,-6.810,M,,*70␍␊
其中GPTRA获取到的是航向信息,GPGGA获取到的是位置信息和差分状态信息,具体每个数字代表的是什么意思,可以查看NEMA协议
在基础的gps导航中,只要获取到小车的航向偏差,位置偏差就可以让小车按照预定的轨迹行走。这其中用到的便是航向与位置。第一步要做的就是把它们发布成话题发布出来。
使用的环境是ubuntu18.04+ros(melodic)。
代码:
gps.msg
Header header #时间包头
#string utc_time #utc时间 eg:073617.90 表示07时36分17.9秒
float64 latitude #纬度
float64 longitude #经度
int32 satellite_num #可搜索卫星数
int32 gps_status # GPS状态
float64 elevation #高程值
float64 time_diff #差分延时
float64 speed #速度
float64 roll_angle #横滚角
float64 heading_angle #航向角
pub_gps.cpp
其中串口号需要根据插入设备在自己机器上的情况进行修改。
可以先通过sudo apt-get install cutecom安装cutecom,再在终端使用sudo cutecom查看
头文件
//头文件
#ifndef _PUB_ANDROID_H
#define _PUB_ANDROID_H
#include "ros/ros.h"
#include "pub_gps/double_gps.h"
#include <sstream>
#include <serial/serial.h>
#include <ctime>
#include <iostream>
#include <cstdlib>
#include <libudev.h>
using namespace std;
tm time_struct = {0};
class GPSParser {
public:
GPSParser();
~GPSParser();
void run();
private:
void parseData(const std::string &data);
private:
ros::NodeHandle nh_;
ros::Publisher pub_gps_data_;
serial::Serial ser_;
ros::Rate loop_rate_;
};
#endif
源文件
#include "pub_gps/pub_Android.h"
GPSParser::GPSParser()
: loop_rate_(100) // 定义定时器的频率为100Hz
{
pub_gps_data_ = nh_.advertise<pub_gps::double_gps>("/GpsData", 100);
std::string port = "/dev/tty_doubleGps"; // 假设串口号为 绑定串口号
unsigned long baudrate = 115200;
try {
ser_.setPort(port);
ser_.setBaudrate(baudrate);
serial::Timeout to = serial::Timeout::simpleTimeout(1000);
ser_.setTimeout(to);
ser_.open();
} catch (serial::IOException &e) {
ROS_ERROR_STREAM("Unable to open port.");
return;
}
if (ser_.isOpen()) {
ROS_INFO_STREAM("Serial Port initialized.");
} else {
return;
}
}
GPSParser::~GPSParser()
{
ser_.close();
}
void GPSParser::run()
{
while (ros::ok()) {
if (ser_.available()) {
std::string raw_data = ser_.readline();
parseData(raw_data);
}
ros::spinOnce();
loop_rate_.sleep(); // 等待定时器到期,以达到设定的频率
}
}
void GPSParser::parseData(const std::string &data)
{
pub_gps::double_gps gps_data;
std::istringstream ss(data);
std::string token;
std::getline(ss, token, ','); // 包头
gps_data.header.stamp = ros::Time::now();
gps_data.header.frame_id = "gps_link";
gps_data.header.seq = 1;
std::getline(ss, token, ','); // utc时间戳
std::getline(ss, token, ','); // 纬度
gps_data.latitude = std::stold(token);
std::getline(ss, token, ','); // 经度
gps_data.longitude = std::stold(token);
std::getline(ss, token, ','); // 卫星数目
gps_data.satellite_num = std::stoi(token);
std::getline(ss, token, ','); // 差分状态
gps_data.gps_status = std::stoi(token);
std::getline(ss, token, ','); // 高程
gps_data.elevation = std::stold(token);
std::getline(ss, token, ','); // 差分延时
gps_data.time_diff = std::stold(token);
std::getline(ss, token, ','); // 速度
gps_data.speed = std::stold(token);
std::getline(ss, token, ','); // 横滚
gps_data.roll_angle = std::stold(token);
std::getline(ss, token); // 航向
gps_data.heading_angle = std::stold(token);
pub_gps_data_.publish(gps_data);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "gps_parser_node");
GPSParser gps_parser;
gps_parser.run();
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(pub_gps)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
sensor_msgs
serial
std_msgs
message_generation
)
add_message_files(
FILES
double_gps.msg
)
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES pub_gps
CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs serial std_msgs message_runtime
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_executable(pub_gps src/pub_gps.cpp)
add_executable(pub_Android src/pub_Android.cpp)
add_dependencies(pub_gps ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(pub_Android ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(pub_gps
${catkin_LIBRARIES}
)
target_link_libraries(pub_Android
${catkin_LIBRARIES}
udev
)
package.xml
<?xml version="1.0"?>
<package format="2">
<name>pub_gps</name>
<version>0.0.0</version>
<description>The pub_gps package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="pc@todo.todo">pc</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/pub_gps</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>serial</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>serial</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>serial</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
我使用的编程环境是vscode。把整个功能包导入自己的工作空间后,由于使用了自定义的msg文件,需要在catkin_make后把build生成的gps.h文件的路径加入到c_cpp_properties.json文件里,具体为:
配置好后使用如下指令可以发布话题
rosrun pub_gps pub_gps
然后查看话题发布和话题的频率,这里我用的是10hz
后续会写怎么用gps+imu控制小车跑起来。