本文记录ROS tutorials 中turtlesim包中teleop_turtle_key的解读
废话少说，直接上代码： https://github.com/ros/ros_tutorials/blob/melodic-devel/turtlesim/tutorials/teleop_turtle_key.cpp
在ROS中路径 ：ros_tutorials/turtlesim/tutorials/teleop_turtle_key.cpp
#include <ros/ros.h>
#include <turtlesim/Velocity.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>
#define KEYCODE_R 0x43
#define KEYCODE_L 0x44
#define KEYCODE_U 0x41
#define KEYCODE_D 0x42
#define KEYCODE_Q 0x71
class TeleopTurtle //声明TeleopTurle类
{
public:
TeleopTurtle(); //构造函数
void keyLoop();
private:
ros::NodeHandle nh_;
double linear_, angular_, l_scale_, a_scale_;
ros::Publisher vel_pub_;
};
TeleopTurtle::TeleopTurtle(): //构造函数的定义，传递默认值
linear_(0),
angular_(0),
l_scale_(2.0),
a_scale_(2.0)
{
nh_.param("scale_angular", a_scale_, a_scale_); // param()和getParam()类似，但是允许指定参数的默认值
// 参考：http://wiki.ros.org/cn/roscpp_tutorials/Tutorials/Parameters
nh_.param("scale_linear", l_scale_, l_scale_); // 同上
vel_pub_ = nh_.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);
// 注册一个话题，话题名为“turtle1/command_velocity”
//消息类型为：turtlesim::Velocity，消息队列大小为1
}
int kfd = 0;
struct termios cooked, raw;
// 定义两个termios结构体，跟Linux终端I/O相关，结构体中是对终端I/O的控制字段
//参看 https://www.jianshu.com/p/bb128a6b413e
void quit(int sig) // 定义一个quit函数
{
tcsetattr(kfd, TCSANOW, &cooked);
//tcsetattr返回终端属性，中间参数表示可以指定在什么时候新的终端属性才起作用
//TCSANOW：更改立即发生
ros::shutdown(); //关闭节点
exit(0); // exit（0）：正常运行程序并退出程序 ,exit（1）：非正常运行导致退出程序；
// 参看：https://www.cnblogs.com/nufangrensheng/archive/2013/03/01/2938508.html
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "teleop_turtle"); // 创建节点的第一步，初始化节点，节点名为："teleop_turtle"
TeleopTurtle teleop_turtle; // 定义一个类
signal(SIGINT,quit); // siganl 函数设置了某个信号的对于动作，再此是进入quit函数
teleop_turtle.keyLoop(); // 调用类中的方法
return(0);
}
void TeleopTurtle::keyLoop()
{
char c;
bool dirty=false;
// get the console in raw mode
tcgetattr(kfd, &cooked); // 参看：https://www.jianshu.com/p/bb128a6b413e
memcpy(&raw, &cooked, sizeof(struct termios)); // 将cooked中的内容拷贝到raw中
raw.c_lflag &=~ (ICANON | ECHO);
//将ICANON和ECHO按位或“|” 之后取反，再将raw.c_lflag和其相与，将结果赋值给raw.c_lflag
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
//raw.c_cc是控制字符 https://www.gwduan.com/web/computer/history/unix-prog/terminal.html#special-char
tcsetattr(kfd, TCSANOW, &raw);
puts("Reading from keyboard"); // puts()函数用来向 标准输出设备 （屏幕）写字符串并换行
puts("---------------------------");
puts("Use arrow keys to move the turtle.");
for(;;) // 循环一直执行 ，知道遇到退出语句
{
// get the next event from the keyboard
//函数从打开的设备或文件中读取数据,ssize_t read(int fd, void *buf, size_t count); 返回值：成功返回读取的字节数，出错返回-1并设置errno，如果在调read之前已到达文件末尾，则这次read返回0
//是请求读取的字节数，读上来的数据保存在缓冲区buf中，同时文件的当前读写位置向后移。注意这个读写
//位置和使用C标准I/O库时的读写位置有可能不同，这个读写位置是记在内核中的，而使用C标准I/O库时的读
//写位置是用户空间I/O缓冲区中的位置。比如用fgetc读一个字节，fgetc有可能从内核中预读1024个字节到I/O
//缓冲区中，再返回第一个字节，这时该文件在内核中记录的读写位置是1024，而在FILE结构体中记录的读
//写位置是1。注意返回值类型是ssize_t，表示有符号的size_t，这样既可以返回正的字节数、0（表示到达文件末尾）也可以返回负值-1（表示出错）
//read函数返回时，返回值说明了buf中前多少个字节是刚读上来的。有些情况下，实际读到的字节数（返回值）会小于请求读的字节数count，例如：读常规文件时，在读到count个字节之前已到达文件末尾。例如，距文件末尾还有30个字节而请求读100个字节，则read返回30，下次read将返回0。
if(read(kfd, &c, 1) < 0)
{
perror("read():");
// perror ( )用 来 将 上 一 个 函 数 发 生 错 误 的 原 因 输 出 到 标 准 设备 (stderr)
exit(-1);
}
linear_=angular_=0;
ROS_DEBUG("value: 0x%02X\n", c);
switch(c) // 根据键盘的按键值，给相应的参数赋值
{
case KEYCODE_L:
ROS_DEBUG("LEFT");
angular_ = 1.0;
dirty = true;
break;
case KEYCODE_R:
ROS_DEBUG("RIGHT");
angular_ = -1.0;
dirty = true;
break;
case KEYCODE_U:
ROS_DEBUG("UP");
linear_ = 1.0;
dirty = true;
break;
case KEYCODE_D:
ROS_DEBUG("DOWN");
linear_ = -1.0;
dirty = true;
break;
}
turtlesim::Velocity vel;
vel.angular = a_scale_*angular_;
vel.linear = l_scale_*linear_;
if(dirty ==true)
{
vel_pub_.publish(vel);
dirty=false;
}
}
return;
}
网友评论