2.ROS基础（创建工作空间，ROS通信编程）

ROS的安装和入门可以遵照如下网址教学：http://wiki.ros.org/cn/kinetic/Installation/Ubuntu

课程内容来源于胡春旭老师的机器人操作系统ROS理论与实践，B站有视频可以观看。重在了解流程！

1.创建工作空间

 这个环境变量的设置只会在当前的终端生效。所以可以往vim ~/.bashrc中加入source ~/catkin_ws/devel/setup.bash，变成全局的环境变量，然后运行source ~/.bashrc，这样关于工作空间的命令在所有终端都可以生效了。

 功能包里有两个重要文件CMakeLists.txt（CMake编译文件）、package.xml（关于功能包的描述），应该注意：编辑功能包的时候应该在工作空间的根目录下进行编译。若添加了新的包，一定要catkin_make进行编译，以创建目录；然后一定要进行source ~/catkin_ws/devel/setup.bash，如此才能确保ROS可以找到任何新的package，message types 和属于新包的Python modules

例如在调用同名的功能包时，系统会从第一个目录开始查找 ，在这里就是图中的/home/hcx/catkin_ws/src，若没有找到则会往后按顺序查找。在添加新的路径时，也会将其自动放置在ROS_PACKAGE_PATH的最前端。

2.ROS通信编程

2.1话题编程

 如何实现一个发布者：

• 初始化节点；
• 向ROS MASTER注册节点信息，包括发布的话题名和话题中的消息类型；
• 按照一定频率循环发布消息。

//in [workspace]/src/[package]/talker.cpp
#include <sstream>
#include "ros/ros.h"
#include "std_msgs/String.h"

int main(int argc,char **argv)
{
	//ROS节点初始化
	ros::init(argc,argv,"talker");
	
	//创建节点句柄（方便管理节点）
	ros::NodeHandle n;

	//创建一个Publisher，发布名为chatter的topic，消息类型为std_msgs::String
	ros::Publisher chatter_pub=n.advertise<std_msgs::String>("chatter",1000);

	//设置循环的频率(Hz)
	ros::Rate loop_rate(10);

	int count = 0;
	while(ros::ok())
	{
		//初始化std_msgs::String类型的消息
		std_msgs::String msg;
		std::stringstream ss;
		ss<<"hello world!"<<count;
		msg.data=ss.str();

		//发布消息
		ROS_INFO("%s",msg.data.c_str());
		chatter_pub.publish(msg);

		//循环等待回调函数
		ros::spinOnce();
		
		//按照循环频率延时，若不延时，CPU占用率会占用特别高
		loop_rate.sleep();
		++count;
	}
	return 0
}
	

如何实现一个订阅者：

• 初始化ROS节点；
• 订阅需要的话题；
• 循环等待话题消息，接收到消息后进入回调函数；
• 在回调函数中完成信息处理。

//listener.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"

//接受订阅的消息后，会进入消息回调函数
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
	//将接收到的消息打印出来
	ROS_INFO("I heard:{%s}",msg->data.c_str());
}

int main(int argc,char **argv)
{
	//初始化节点
	ros::init(argc,argv,"listener");
	
	//创建节点句柄
	ros::NodeHandle n;
	
	//创建一个Subscriber，订阅名为chatter的topic，注册回调函数chatterCallback
	//回调函数的机制，因为subscriber不知道何时有数据进来，所以会通过多线程的方式在后台一直等待数据的进来
	ros::Subscriber sub = n.subscribe("chatter",1000,chatterCallback);
	
	//循环等待回调函数
	ros::spin();
	
	return 0;
}

如何编译代码：

• 设置需要编译的代码和生成的可执行文件；
• 设置链接库；
• 设置依赖。

若为python则不用配置，因为它本身就是可执行的脚本。

##in  [workspace]/src/[package]/CMakeLists.txt
add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})

然后在工作空间根目录下catkin_make进行编译。

接着就可以rosrun [package] [可执行文件] 来运行可执行文件了。

那如何自定义话题信息呢：

然后在wokspace进行编译，然后可以在终端进行查看是否编译成功：

 2.2服务编程

 

 如何实现一个服务器：

• 初始化ROS节点；
• 创建Server实例；
• 循环等待服务请求，进入回调函数；
• 在回调函数中完成服务功能的处理，并反馈应答数据。

#include "ros/ros.h"
#include "learning_communication/AddTwoInts.h"

//service回调函数，输入参数req，输出参数res
bool add(learning_communication::AddTwoInts::Request &req, learning_communication::AddTwoInts::Request &res)
{
	//将输入参数中的请求数据相加，结果放到应答变量中
	res.sum=req.a+req.b;
	ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
	ROS_INFO("sending back response: [%ld]", (long int)res.sum);
	
	return res;
}

int main(int argc, char **argv)
{
	//ROS节点初始化
	ros::init(argc, argv, "add_two_int_server");
	
	//创建节点句柄
	ros::NodeHandle n;
	
	//创建一个名为add_two_inits的server，注册回调函数add()
	ros::ServiceServer service = n.advertiseService("add_two_ints", add);
	
	//循环等待回调函数
	ROS_INFO("Ready to add two ints.");
	ros::spin();
	
	return 0;
}

如何实现一个客户端：

• 初始化节点；
• 创建一个Client实例；
• 发布服务请求数据；
• 等待Server处理之后的应答结果。

#include <cstdlib>
#include "ros/ros.h"
#include "learning_communication/AddTwoInts.h"

int main(int argc, char **argv)
{
	//ROS节点初始化
	ros::init(argc, argv ,"add_two_ints_client");
	
	//从终端命令行获取两个加数
	if (argc != 3)
	{
		ROS_INFO("usage: add_two_ints_client X Y");
		return 1;
	}
	
	//创建节点句柄
	ros::NodeHandle n;
	
	//创建一个client，请求add_two_int service，service消息类型learning_communication::AddTwoInts
	ros::ServiceClient client = n.serviceClient<learning_communication::AddTwoInts>("add_two_ints");
	
	//创建learning_communication::AddTwoInts类型的service消息
	learning_communication::AddTwoInts srv;
	srv.request.a=atoll(argv[1]);
	srv.request.b=atoll(argv[2]);
	
	//发布service请求，等待加法运算的应答结果
	if (client.call(srv))
	{
		ROS_INFO("Sum: %ld", (long int)srv.response.sum);
	}
	else
	{
		ROS_ERROR("Failed to call service add_two_ints");
		return 1;
	}
	return 0;
}

如何编译代码：

add_executable(server src/server.cpp)
target_link_libraries(server ${catkin_LIBRARIES})
add_dependencies(server ${PROJECT_NAME}_gencpp)

add_executable(client src/client.cpp)
target_link_libraries(client ${catkin_LIBRARIES})
add_dependencies(client ${PROJECT_NAME}_gencpp)

运行后如下：

3.3动作编程

 

 如何实现一个动作服务器：

• 初始化节点；
• 创建动作服务器实例；
• 启动服务器，等待动作请求；
• 在回调函数中完成动作服务处理，并反馈进度信息；
• 动作完成，发送结束信息。

#include "ros/ros.h"
#include <actionlib/server/simple_action_server.h>
#include "learning_communication/DoDishesAction.h"

typedef actionlib::SimpleActionServer<learning_communication::DoDishesAction> Server;

//收到action的goal后启用该回调函数
void execute(const learning_communication::DoDishesGoalConstPtr& goal,Server* as)
{
	ros::Rate r(1);
	learning_communication::DoDishesFeedback feedback;
	
	ROS_INFO("Dishwasher %d is working.", goal->dishwasher_id);
	
	//假设洗盘子的进度，并且按照1Hz的频率发布进度feedback
	for(int i=1; i<=10; i++)
	{
		feedback.percent_complete = i * 10;
		as->publishFeedback(feedback);
		r.sleep();
	}
	
	//当action完成后，向客户端返回结果
	ROS_INFO("Dishwasher %d finish worfking.", goal->dishwasher_id);
	as->setSucceeded();
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "do_dishes_server");
	ros::NodeHandle n;
	
	//定义一个服务器
	Server server(n, "do_dishes", boost::bind(&execute, _1, &server), false);
	
	//服务器开始运行
	server.start();
	
	ros::spin();
	
	return 0;
}

如何实现一个动作客户端：

• 初始化ROS节点；
• 创建动作客户端实例；
• 连接动作服务端；
• 发送动作目标；
• 根据不同类型的服务端反馈处理回调函数。
   

#include "ros/ros.h"
#include <actionlib/client/simple_action_client.h>
#include "learning_communication/DoDishesAction.h"

typedef actionlib::SimpleActionClient<learning_communication::DoDishesAction> Client;

//当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state, const learning_communication::DoDishesResultConstPtr& result)
{
	ROS_INFO("yay! The dishes are now clean");
	ros::shutdown();
}

//当action激活后调用该回调函数
void activeCb()
{
	ROS_INFO("Goal just went active");
}

//收到feedback后调用该回调函数
void feedbackCb(const learning_communication::DoDishesFeedbackConstPtr& feedback)
{
	ROS_INFO(" percent_complete : %f",feedback->percent_complete);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "do_dishes_client");
	
	//定义一个客户端
	Client client("do_dishes", true);
	
	//等待服务器端
	ROS_INFO("Waiting for action server to start.");
	client.waitForServer();
	ROS_INFO("Action server started, sending goal.");
	
	//创建一个action的goal
	learning_communication::DoDishesGoal goal;
	goal.dishwasher_id=1;
	
	//发送action的goal给服务器端，并设置回调函数
	client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
	
	ros::spin();
	
	return 0;
}
	

如何编译代码：

• 设置需要编译的代码和生成的可执行文件；
• 设置链接库；
• 设置依赖。

add_executable(DoDishes_client src/DoDishes_client.cpp)
target_link_libraries(DoDishes_client ${catkin_LIBRARIES})
add_dependencies(DoDishes_client ${${PROJECT_NAME}_EXPORTED_TARGETS})

add_executable(DoDishes_server src/DoDishes_server.cpp)
target_link_libraries(DoDishes_server ${catkin_LIBRARIES})
add_dependencies(DoDishes_server ${${PROJECT_NAME}_EXPORTED_TARGETS})