SLAM从入门到精通（ROS应用开发）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        前面我们陆陆续续学习了关于ros的一些知识点，比如如何创建包、如何进行消息访问、如何注册服务、如何访问参数等等。那么对于我们使用者来说，应该如何进行ros开发呢？其实就是把之前的功能整合起来，那就是一个基本的ros应用了。

1、创建头文件book_class.h

 
#ifndef _BOOK_CLASS_H
#define _BOOK_CLASS_H
 
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float64.h"
#include "std_msgs/Int32.h"
#include "beginner_tutorials/PassWord.h"
 
using namespace std;
 
class RosBasics
{
	public:
		RosBasics();
		RosBasics(ros::NodeHandle& nh);
		~RosBasics();
		void initPublish();
		void initSubscribe();
		void initServer();
		bool serverCallback(beginner_tutorials::PassWord::Request& req,
				beginner_tutorials::PassWord::Response& res);
		void msgCallback(const std_msgs::Float64& stdmsg);
 
		void paramTest();
 
	private:
		ros::NodeHandle nh_;
		ros::Publisher pub_;
		ros::Subscriber sub_;
		ros::ServiceServer serv_;
};
#endif

        头文件一般是为了其他模块方便调用而创建的。如果应用比较简单，不创建头文件也是可以的。

2、创建book_class.cpp文件

 
#include "book_class.h"
 
RosBasics::RosBasics():nh_("~")
{
	initPublish();
	initServer();
	initSubscribe();
	paramTest();
}
 
RosBasics::RosBasics(ros::NodeHandle& nh):nh_(nh)
{
	initPublish();
	initServer();
	initSubscribe();
	paramTest();
}
 
RosBasics::~RosBasics()
{
	return;
}
 
void RosBasics::initPublish()
{
	ROS_INFO("publish initializing!");
	pub_ = nh_.advertise<std_msgs::Float64>("Topic2", 100);
}
 
void RosBasics::initSubscribe()
{
	ROS_INFO("subscribe initializing");
	sub_ = nh_.subscribe("Topic1", 1, &RosBasics::msgCallback, this);
}
 
void RosBasics::initServer()
{
	ROS_INFO("server initializing");
	serv_ = nh_.advertiseService("pwdcheck", &RosBasics::serverCallback, this);
}
 
bool RosBasics::serverCallback(beginner_tutorials::PassWord::Request& req,
				beginner_tutorials::PassWord::Response& res)
{
	res.result = (req.password == 123456) ? true: false;
	if(res.result)
	{
		ROS_INFO("Welcom, password correct!");
	}
	else
	{
		ROS_INFO("Sorry, password error");
	}
	return true;
}
 
void RosBasics::msgCallback(const std_msgs::Float64& stdmsg)
{
	std_msgs::Float64 msgpub;
	msgpub.data = stdmsg.data + 2;
	ROS_INFO("Receive date from Topic1 content is:%f", stdmsg.data);
	ROS_INFO("Publish date with Topic2 content is:%f", msgpub.data);
	pub_.publish(msgpub);
}
 
void RosBasics::paramTest()
{
	int Age;
	bool checkAge;
	checkAge = nh_.getParam("Age", Age);
	if(checkAge)
	{
		ROS_INFO("Welcom, Get Param Success");
		if(Age > 18)
		{
			ROS_INFO("You are adult!");
		}
		else
		{
			ROS_INFO("Your are children!");
		}
	}
	else
	{
		ROS_INFO("Sorry, Get Param Fail!");
	}
}

        整个应用类名叫RosBasics。初始化的部分主要是构造函数RosBasics()，里面有四个基本动作，分别是initPublish、initServer、initSubscribe、paramTest。除了paramTest是为了验证参数设置的，其他的函数都是为了发布某种信息，或者是订阅某种消息。除了这些函数之外，还有两个函数比较重要，分别是serverCallback和msgCallback，前者是服务响应的时候被调用，后者是消息订阅的时候被响应。

3、添加main函数

int main(int argc, char* argv[])
{
	ros::init(argc, argv, "node_classdemo");
	ros::NodeHandle nh;
	RosBasics classdemo(nh);
	ros::spin();
	return 0;
}

4、添加编译脚本

add_executable(book_class src/book_class.cpp)
target_link_libraries(book_class ${catkin_LIBRARIES})
add_dependencies(book_class beginner_tutorials_generate_messages_cpp)

        添加了编译脚本之后，就可以使用catkin_make命令去处理了。

5、测试

        测试之前首先启动roscore、初始化./devel/setup.sh环境。接着就是启动book_class应用，即，

feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosrun beginner_tutorials book_class
[ INFO] [1695373247.361995894]: publish initializing!
[ INFO] [1695373247.362640551]: server initializing
[ INFO] [1695373247.362935090]: subscrbe initializing
[ INFO] [1695373247.363917937]: Sorry, Get Param Fail!

        针对这个程序，测试一般有三个。第一，通过rosparam增加Age参数，重启book_class，查看param是否发生改变，

feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws/src/beginner_tutorials/src$ rosparam set Age 18
Cfeixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosrun beginner_tutorials book_class
[ INFO] [1695373270.010051953]: publish initializing!
[ INFO] [1695373270.010738103]: server initializing
[ INFO] [1695373270.011012052]: subscrbe initializing
[ INFO] [1695373270.011977071]: Welcom, Get Param Success
[ INFO] [1695373270.012022883]: Your are children!

        第二，利用rosservice调用pwdcheck，判断是否有对应打印，

feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws/src/beginner_tutorials/src$ rosservice call pwdcheck 123456
result: True

        第三，用rostopic发布Topic1消息，查看是否有对应的打印，

feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws/src/beginner_tutorials/src$ rostopic pub -r 10 /Topic1 std_msgs/Float64  1
feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosrun beginner_tutorials book_class
[ INFO] [1695373395.125181843]: publish initializing!
[ INFO] [1695373395.125854890]: server initializing
[ INFO] [1695373395.126141408]: subscrbe initializing
[ INFO] [1695373395.127212501]: Welcom, Get Param Success
[ INFO] [1695373395.127267569]: Your are children!
[ INFO] [1695373410.061866408]: Welcom, password correct!
[ INFO] [1695373443.247473790]: Receive date from Topic1 content is:1.000000
[ INFO] [1695373443.247526881]: Publish date with Topic2 content is:3.000000

6、关于launch文件的小技巧

        经过之前的学习我们知道，如果node节点比较多，那么一个一个起来很麻烦。这个时候，我们就可以通过写一个launch文件，来解决这个问题。如下面脚本所示，有这么一个beginner.launch这个文件，

​
<launch>
  <node pkg="beginner_tutorials" type="listener" name="listener" />
</launch>
 
​

        这个时候，就可以输入roslaunch beginner.launch文件，就可以直接验证了。如果它发现roscore没有启动，也会帮助我们先把roscore启动起来的。当然，如果launch文件时保存在package下面，那么启动的时候，带上package名字就好了，

rosrun beginner_tutorials beginner.launch

        其他需要注意的是，

1）arg相当于公共变量；
2）node就是package中的程序，提供package和type就行；
3）param是参数，放在全局就是全局变量，node里面就是私有变量；
可以通过ros::param::param<string>(“data_type”, data_type, “images”)访问
其中data_type是string类型，如果没有，“images”是默认类型，有就读取。
如果是私有类型，param放在node中间；
ros::param::param<string>(“~data_type”, data_type, “images”)
4）include嵌套其他launch文件。