ROS+P3DX代码注解 [ 1 ] -- RosAria_client代码注解
首先当拿到源码的时候，应该先从CMakeLists.txt看起来，然后在看对应的源码。
#cmake最小支持的版本
cmake_minimum_required(VERSION 2.8.3)
#项目的名称
project(rosaria_client)
#添加一些package的
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
geometry_msgs
rosaria
)
catkin_package()
#包含的子目录
include_directories(
${catkin_INCLUDE_DIRS}
)
## Declare a cpp executable
#然后是添加子文件，前面的地方执行文件的名称，后面的是这个cpp的源文件
add_executable(go_three_second src/go_three_second.cpp)
add_executable(spin_counterclockwise src/spin_counterclockwise.cpp)
add_executable(spin_clockwise src/spin_clockwise.cpp)
add_executable(teleop src/teleop.cpp)
add_executable(print_state src/print_state.cpp)
add_executable(interface src/interface.cpp)
add_executable(enable_motors src/enable_motors.cpp)
#然后就是将每个Cpp文件分别连接库
target_link_libraries(go_three_second
${catkin_LIBRARIES}
)
target_link_libraries(spin_counterclockwise
${catkin_LIBRARIES}
)
target_link_libraries(spin_clockwise
${catkin_LIBRARIES}
)
target_link_libraries(teleop
${catkin_LIBRARIES}
)
target_link_libraries(print_state
${catkin_LIBRARIES}
)
target_link_libraries(interface
${catkin_LIBRARIES}
)
target_link_libraries(enable_motors
${catkin_LIBRARIES}
)总结：
cmakelists.txt的总体的框架
#cmake最小支持的版本
cmake_minimum_required(VERSION 2.8.3)
#项目的名称
project(rosaria_client)
#添加一些package的
find_package(catkin REQUIRED COMPONENTS
roscpp
)
catkin_package()
#包含的子目录
include_directories(
${catkin_INCLUDE_DIRS}
)
#然后是添加子文件，前面的地方执行文件的名称，后面的是这个cpp的源文件
#add_executable(执行文件的名称 从项目主要目录找到cpp)
add_executable(go_three_second src/go_three_second.cpp)
#然后就是将每个Cpp文件分别连接库
target_link_libraries(go_three_second
${catkin_LIBRARIES}
)enable_motors.cpp：
/*
作用：启动电机
*/
#include spin_clockwise.cpp:
/*
作用：让车旋转45度后停下来（将角速度和线速度都变成0）
*/
#include("RosAria/cmd_vel", 1000);
//实例化一个geometry_msgs命名空间下的Twist对象，这个对象的名字就叫做msg
geometry_msgs::Twist msg;
//设定参数，最开始的速度，运动的时间持续时间，角速度 PI
double BASE_SPEED = 0.2, MOVE_TIME = 3.0, CLOCK_SPEED = 0.5, PI = 3.14159;
//判断旋转的
int count = 0;
//实例化ros命名空间下的Rate对象，对这个对象的进行初始化，将速度赋值进去
ros::Rate rate(CLOCK_SPEED);
// Make the robot stop (robot perhaps has a speed already)
//为了防止意外，让车的初速度设置为0,这里的原始是根据现实出msg 的数据类型
msg.linear.x = 0;
msg.linear.y = 0;
msg.linear.z = 0;
msg.angular.x = 0;
msg.angular.y = 0;
msg.angular.z = 0;
pub.publish(msg);
/*
ros::ok()这个函数用于当你按下ctrl+c的时候就会返回false，因此经常用于while,或者其他判断语句当中
或者当MOVE_TIME/CLOCK_SPEED +1 这个数值不大于0的时候，首先这里是想要旋转6次，然后count的自己+1就，
后面这个部分是一个定值，就是说7.那么就让count循环了6次。
*/
while(ros::ok() && count1)
{
// Spin PI/4
/*
修改msg.angular.z的数值，根据那个结构，然后发布这个消息，
这里的move_time/clock_speed 就是让那他每秒转pi/4的6分一。那么旋转6秒
这个旋转的框架还是和厉害的。
*/
if (count == 0 || count == 1)
{
msg.angular.z = -1 * PI/ int(MOVE_TIME/CLOCK_SPEED) / 4;
pub.publish(msg);
}
//向屏幕端输出下面的信息
ROS_INFO_STREAM("The robot is now spinning clockwise!");
//然后这里进行自加1
count++;
//调用ros命名空间下的spionOnce()函数，如果这个程序当中有订阅者，那么这条语句就必须存在，为什么在这里写，因为咋这里旋转啊
ros::spinOnce();
//休眠一下，默认是每个1HZ,也就是说1秒刷新一次。也就是说，程序到时候刷新几秒
rate.sleep();
}
// Stop the spin停止旋转，并且将原来的线速度都变成0，并且将速度为0这个消息在发布到msg当中，这里为什么要用一个for语句循环两侧，我就不知道了
for(int i = 0; i < 2; i++)
{
msg.angular.x = 0;
msg.angular.y = 0;
msg.angular.z = 0;
pub.publish(msg);
}
//输出辅助信息
ROS_INFO_STREAM("The robot finished spinning 45 degrees!");
// Guard, make sure the robot stops保证这个时候的线速度也是0,并且进行过发布
rate.sleep();
msg.linear.x = 0;
msg.linear.y = 0;
msg.linear.z = 0;
pub.publish(msg);
} teleop.cpp:
/*
功能：实现箭头控制，以及ctrl+c表示暂停
*/
#include ("RosAria/cmd_vel", 1);
}
//这是帧率
int kfd = 0;
//定义结构体
struct termios cooked, raw;
/*
补充：
这个函数包含在 termios.h头文件中
tcsetarr(int fd, int optional_action,const struct)函数用于设置终端参数
另外const * 在使用的时候，就是& 一一对应就好
参数列表： fd 是帧率 optional_action用于控制修改起作用的时间 strcut保存要修改的参数
option_action:
TCSANOW：不等待数据传输完毕就立即改变属性
*/
/*
暂停函数，全局函数
*/
void quit(int sig)
{
tcsetattr(kfd, TCSANOW, &cooked);
ros::shutdown();//如果按下ctrl+c立即退出
exit(0);
}
//函数主线程的入口
int main(int argc, char** argv)
{
ros::init(argc, argv, "teleop_RosAria");
TeleopRosAria teleop_RosAria;
/*
补充：signal()函数在头文件 signal.h中，
signal(参数1,参数2)
参数1:我们要进行处理的信信号
参数2:我们要处理的方式
SIGINT是由于interrupt Key产生
quit是一个全局函数
*/
//设置某一个信号对应的动作，这里设置的应该是强制结束的动作。
signal(SIGINT,quit);
//调用keyloop函数
teleop_RosAria.keyLoop();
return(0);
}
//功能：键盘控制
void TeleopRosAria::keyLoop()
{
char c;
bool dirty=false;
// get the console in raw mode 得到控制的初始模型
/*
补充：tcgetattr函数在termios.h头文件中，用于获取终端的相关参数 kdf是一个全局变量
memcpy函数在string.h文件中，用于从src指定的位置拷贝N个字节到目标dest所指的内存地址的起始的位置
memcpy(src,dest,n)
raw是一个结构体，表示：
结构体的c_flag表示本地模式标志说明：
ICANON说明是启动标准模式
ECHO应该受到一个ERASE控制符的时候删除前一个显示符号，我觉着这个地方是为了箭头做准备的保护机制。
*/
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
// Setting a new line, then end of file 在文件后面设置一个新的线程
/*
c_cc[VEOF]默认的控制符号 D，在标准模式下，使用read()返回0 ,标志一个文件结束
c_cc[VEOL]在行尾加上一个换行符号（"\n"）标志一个行的结束，重新开始一行
*/
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
//输出辅助信息
puts("Reading from keyboard");
puts("---------------------------");
puts("Use arrow keys to move the robot.");
puts("Press the space bar to stop the robot.");
puts("Press q to stop the program");
//死循环
for(;;)
{
// get the next event from the keyboard 从键盘得到下一个事件
if(read(kfd, &c, 1) < 0)
{
perror("read():");//将上一个函数发生错误的原因输出到设备参数中
exit(-1);
}
//线速度的角度速度为0
linear_=angular_=0;
//信息在程序中运行不直接显示到终端上
ROS_DEBUG("value: 0x%02X\n", c);
//箭头控制
switch(c)
{
case KEYCODE_L:
ROS_DEBUG("LEFT");
angular_ = 0.1;
linear_ = 0;
dirty = true;
break;
case KEYCODE_R:
ROS_DEBUG("RIGHT");
angular_ = -0.1;
linear_ = 0;
dirty = true;
break;
case KEYCODE_U:
ROS_DEBUG("UP");
linear_ = 0.1;
angular_ = 0;
dirty = true;
break;
case KEYCODE_D:
ROS_DEBUG("DOWN");
linear_ = -0.1;
angular_ = 0;
dirty = true;
break;
case KEYCODE_SPACE:
ROS_DEBUG("STOP");
linear_ = 0;
angular_ = 0;
dirty = true;
break;
case KEYCODE_Q:
ROS_DEBUG("QUIT");
ROS_INFO_STREAM("You quit the teleop successfully");
return;
break;
}
//实例化对象
geometry_msgs::Twist twist;
//这个里面angular里面肯定是有继承关系的
twist.angular.z = a_scale_*angular_;
twist.linear.x = l_scale_*linear_;
//
if(dirty ==true)
{
/*
只是发布一次消息
*/
twist_pub_.publish(twist);
dirty=false;
}
}
//return 相当与break吧
return;
} print_state.cpp：
/*
功能：输出先锋机器人状态的信息
*/
#include // for std :: setprecision and std :: fixed
int battery_msg_count = 0, bumper_msg_count = 0;//初始化保险杠的距离和电池的状态
/*
补充：setprecision 控制输出流显示的浮点数的有效数字的个数（会有四舍五入） 需要加入头文件iomanip.h
std::fix表示用一般的方式输出浮点数
*/
/* checks pose messages and outputs them to user
输出当前位置的和角度，用浮点数输出
*/
void poseMessageReceived(const nav_msgs::Odometry& msg)
{
std::cout << std::setprecision(2) << std::fixed << /* output the pose information using standard output */
"Current position=(" << msg.pose.pose.position.x << ", " << msg.pose.pose.position.y << ") " <<
"Current direction=" << std::setprecision(2) << std::fixed << msg.pose.pose.orientation.w<<"\r";
std::flush(std::cout);
}
/* output the state of the bumpers using rosaria
使出保险杠的状态
*/
void bumperStateMessageReceived(const rosaria::BumperState &msg)//这个地方是一个形参
{
int front_size, rear_size; //定义前面的距离和后面的剧烈
if (bumper_msg_count == 0)
{
// ROS_INFO_STREAM("The front bumpers are "<
front_size = sizeof(msg.front_bumpers) / sizeof(bool);
rear_size = sizeof(msg.rear_bumpers) / sizeof(bool);
ROS_INFO_STREAM("The front bumpers state are('1' means good): ");//输出辅助信息
std::cout << " ";
for (int i=0;iif (msg.front_bumpers[i])
std::cout<<'1';
else
std::cout<<'0';
//换行
std::cout<<std::endl;
ROS_INFO_STREAM("The rear bumpers state are('1' means good): ");
std::cout << " ";
for (int i=0;iif (msg.rear_bumpers[i])
std::cout<<'1';
else
std::cout<<'0';
std::cout<<std::endl;
bumper_msg_count++;
}
}
/* check the status of the battery charge 检查电池的状态 分别对应msg的类型，然后输出就可以啦*/
void batteryStateOfChargeMessageReceived(const std_msgs::Float32 msg)
{
// Right now this feature is not included in the pioneer-3 robot
ROS_INFO_STREAM("The battery state of charge is "</* check + output the voltage of the battery using standard output 检查电压的状态 */
void batteryVoltageMessageReceived(const std_msgs::Float64 msg)
{
if (battery_msg_count == 0)
{
battery_msg_count++;
ROS_INFO_STREAM("The battery voltage is "<< msg);
}
}
void batteryChargeStateMessageReceived(const std_msgs::Int8 msg)
{
// Right now this feature is not included in the pioneer-3 robot
ROS_INFO_STREAM("The battery charge state message received is "<< msg);
}
/* check the state of the motor and output using standard output 检查发动机的状态*/
void motorsStateMessageReceived(const std_msgs::Bool msg)
{
if (msg.data)
ROS_INFO_STREAM("The motor is good");
else
ROS_INFO_STREAM("The motor is bad");
}
/* call all of the functions implemented above and provide user with robot state info */
int main(int argc, char **argv)
{
// Initialize the ROS system and become a node.初始化
ros::init(argc, argv, "print_aria_state"); ros::NodeHandle nh;
// Create a subscriber object .
//就爱嗯这个订阅各自的话题
ros::Subscriber pose, bumper_state, battery_state_of_charge, battery_voltage, battery_charge_state, motors_state;
pose = nh.subscribe("RosAria/pose", 1000, &poseMessageReceived) ; //supply pose
bumper_state = nh.subscribe("RosAria/bumper_state", 1000, &bumperStateMessageReceived) ; //inform bumper state
battery_state_of_charge = nh.subscribe("RosAria/bumper_state_of_charge", 1000, &batteryStateOfChargeMessageReceived) ; //inform state of charge
battery_voltage = nh.subscribe("RosAria/battery_voltage", 1000, &batteryVoltageMessageReceived) ; //inform battery voltage level
battery_charge_state = nh.subscribe("RosAria/battery_charge_state", 1000, &batteryChargeStateMessageReceived) ; //inform charge state
motors_state = nh.subscribe("RosAria/motors_state", 1000, &motorsStateMessageReceived) ; //inform motor state
// Let ROS take over.悬停
ros::spin();
} RosAria_client.launch文件
"rosaria" type = "RosAria" name = "RosAria">
"port" value="/dev/ttyUSB0" type="string"/>
"rosaria_client" type="interface" name="RosAria_interface" output="screen"/>
在launch文件中添加节点和节点名称，以及对一些参数的修改