学习ROS也有一个月了,起初看了重德智能的一些视频,然后又看了古月老师的9讲视频(都在B站找的),边看跟着边操作,但是看完之后也忘得差不多了。现在写个里程计的程序,都扭扭捏捏下不了手。终于经过几天的软磨硬泡,写了这一段,在此仅作为记录学习的过程。如果有错误和不足,也希望大家帮忙指出,提出自己的意见,共同进步吧。
首先,还没有写 launch 文件,就直接从main 函数开始记录。
//接收其他节点的速度和角度
double Robot_Speed = 0;
double Robot_Angle = 0;
void cmdCallback(const geometry_msgs::Twist& msg)
{
//以线速度的X方向的速度表示整体车行速度
Robot_Speed = msg.linear.x;
//以角速度的Z方向的速度表示整体车行角度
Robot_Angle = msg.angular.z;
}
int main(int argc, char** argv)
{
//初始化ROS节点
ros::init(argc, argv, "comm_bringup");
//创建节点句柄
ros::NodeHandle nh;
//初始化comm_odom
comm_odom::comm_odom car;
if(!car.init())
ROS_ERROR("comm odom initialized failed.");
ROS_INFO("comm odom initialized successful.");
ros::Subscriber sub = nh.subscribe("cmd_vel", 50, cmdCallback);
//循环运行
ros::Rate loop_rate(50);
while (ros::ok())
{
ros::spinOnce();
//机器人控制
car.spinOnce(Robot_Speed, Robot_Angle);
loop_rate.sleep();
}
return 0;
}
创建了一个新的 命名空间 comm_odom::comm_odom car; 这个 car 可以从头文件里面去查看具体的定义。
然后看到 car.init() 这个函数如下,就是初始化 串口的参数,这个串口用的 boost ,好像还有个用 serial 的包,不知道这两个是什么区别。暂时先跟着用的boost 的串口。代码如下,
bool comm_odom::init()
{
//串口连接
sp.set_option(boost::asio::serial_port::baud_rate(115200));
sp.set_option(boost::asio::serial_port::flow_control(boost::asio::serial_port::flow_control::none));
sp.set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));
sp.set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::one));
sp.set_option(boost::asio::serial_port::character_size(8));
ros::Time::init();
current_time_ = ros::Time::now();
last_time_ = ros::Time::now();
//定义发布消息的名称
pub_ = nh.advertise<nav_msgs::Odometry>("odom", 50);
return true;
}
其中,先全局定义了串口的设备
//串口准备
boost::asio::io_service iosev;
boost::asio::serial_port sp(iosev, "/dev/ttyUSB0");
如果检测到了串口并连接上了,就进入下面的循环了。循环主要就是处理串口的一些数据了,包括串口发送速度,接收速度解析等。进入下面这个函数。
//机器人控制
car.spinOnce(Robot_Speed, Robot_Angle);
//机器人控制 通信
bool comm_odom::spinOnce(double Robot_S, double Robot_A)
{
//下发机器人期望速度
writeSpeed(Robot_S, Robot_A);
//读取机器人实际速度
readSpeed();
current_time_ = ros::Time::now();
//发布TF
geometry_msgs::TransformStamped odom_trans;
odom_