ROS2_Foxy学习6——进阶编程_C++

里面的例子参考 官方教程，然后附带一些解释和一些推荐的便于理解的文章。

1 编写 action

新建工作空间，并新建两个功能包：动作消息功能包和动作功能包。动作消息功能包中编写action文件，然后在动作功能包中使用前者的action接口。

1.1 自定义消息 action

1、创建：在动作消息功能包（这里包名是action_tutorials_interfaces）下，创建/action文件夹（与/src同级），并在其中建立.action消息文件，有关消息内容，详见本系列第三篇《ROS2_Foxy学习（三）核心概念》。

Fibonacci.action

int32 order
---
int32[] sequence
---
int32[] partial_sequence

2、在package.xml文件中添加

	<buildtool_depend>rosidl_default_generators</buildtool_depend>
	<depend>action_msgs</depend>
	<member_of_group>rosidl_interface_packages</member_of_group>

注：action_msgs必要。
3、在CMakeLists.txt文件中添加

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "action/Fibonacci.action"
)

4、消息文件测试
功能包编译后（没有节点，没有运行），可以通过命令ros2 interface show来查看消息文件。

$ ros2 interface show action_tutorials_interfaces/action/Fibonacci

---

注 ：下面创建的动作功能包名是action_tutorials_cpp，附带以下依赖，其中第一个就是刚刚创建的自定义消息功能包，可以在创建包时，通过参数dependencies添加，也可以手动修改package.xml（< depend >）和CMakeLists.txt（find_package）。

action_tutorials_interfaces 
rclcpp 
rclcpp_action 
rclcpp_components

1.2 动作服务端 action server

在动作功能包的/src目录下，编写动作服务端节点，fibonacci_action_server.cpp

#include <functional>
#include <memory>
#include <thread>

#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

//#include "action_tutorials_cpp/visibility_control.h"

namespace action_tutorials_cpp
{
class FibonacciActionServer : public rclcpp::Node
{
public:
	using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
	using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
	
	//构造函数：
	//1、初始化动作服务节点名 fibonacci_action_server
	//2、初始化动作服务器：消息类型Fibonacci，所属节点this，动作名fibonacci，回调函数
	explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : Node("fibonacci_action_server", options)
	{
	using namespace std::placeholders;

	this->action_server_ = rclcpp_action::create_server<Fibonacci>(	//消息类型Fibonacci
	this,									//所属节点this，
	"fibonacci",								//动作名fibonacci
	std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),	//回调函数：handle_goal  	接受目标
	std::bind(&FibonacciActionServer::handle_cancel, this, _1),		//回调函数：handle_cancel   	接受目标取消
	std::bind(&FibonacciActionServer::handle_accepted, this, _1));	//回调函数：handle_accepted 	接受新目标并处理
	}

private:
	rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;

	//接受目标，同时告知client，这边知晓目标了，告知client的过程被自动处理了，下同
	rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const Fibonacci::Goal> goal)
	{
		RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
		(void)uuid;
		return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
	}

	//接受目标取消，同时告知client，这边知道取消了
	rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
	{
		RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
		(void)goal_handle;
		return rclcpp_action::CancelResponse::ACCEPT;
	}

	//接受新目标并处理
	void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
	{
		using namespace std::placeholders;
		
		//因为处理过程是一个耗时且长期运行的操作，因此将其移到另一线程处理
		std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
	}

	//处理并反馈
	void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
	{
		RCLCPP_INFO(this->get_logger(), "Executing goal");
		
		//处理频率 1Hz
		rclcpp::Rate loop_rate(1);
		
		//获取目标
		const auto goal = goal_handle->get_goal();
		
		//定义反馈
		auto feedback = std::make_shared<Fibonacci::Feedback>();
		auto & sequence = feedback->partial_sequence;
		sequence.push_back(0);	//斐波那契数列的前两个值是 0 1	动作服务器将不断反馈当前的sequence到客户端
		sequence.push_back(1);
		
		//定义结果
		auto result = std::make_shared<Fibonacci::Result>();

		//计算
		for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) 
		{
			//检查目标是否被取消
			if (goal_handle->is_canceling()) 
			{
				result->sequence = sequence;	
				goal_handle->canceled(result);	
				RCLCPP_INFO(this->get_logger(), "Goal canceled");
				return;
			}
			
			//更新sequence
			sequence.push_back(sequence[i] + sequence[i - 1]);
			
			//反馈sequence
			goal_handle->publish_feedback(feedback);
			RCLCPP_INFO(this->get_logger(), "Publish feedback");

			loop_rate.sleep();
		}

		//目标达成
		if (rclcpp::ok()) 
		{
			result->sequence = sequence;
			goal_handle->succeed(result);
			RCLCPP_INFO(this->get_logger(), "Goal succeeded");
		}
	}
};  // class FibonacciActionServer

}  // namespace action_tutorials_cpp

RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)

注：
1、goal_handle：关于目标句柄goal_handle（也就是rclcpp_action::ServerGoalHandle），可以看下官方的帮助文档，这里刚学，还不是很理解，写多了也许就明白了。
2、std::thread：看这里，有关join和detach可以瞧一瞧这里。
3、这里没有使用main函数，而是用下面这句，表明该节点可以在运行时动态加载，看看官方帮助文档，这里也不是很理解，后面再学习。

RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)

1.3 动作客户端 action client

在动作功能包的/src目录下，编写动作客户端节点，fibonacci_action_client.cpp

#include <functional>
#include <future>	//C++11 异步通信
#include <memory>
#include <string>
#include <sstream>	//C++标准库中 sstream 提供了比ANSI C的 stdio 更高级的一些功能，即单纯性、类型安全和可扩展性。

#include "action_tutorials_interfaces/action/fibonacci.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

namespace action_tutorials_cpp
{
class FibonacciActionClient : public rclcpp::Node
{
public:
	using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
	using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;

	//构造函数：
	//1、初始化动作客户端节点名 fibonacci_action_client
	//2、初始化动作客户端：动作类型Fibonacci、所属节点this、动作名fibonacci
	//3、初始化定时器，每500ms发送一次目标
	explicit FibonacciActionClient(const rclcpp::NodeOptions & options) : Node("fibonacci_action_client", options)
	{
		this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
		this,
		"fibonacci");

		this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&FibonacciActionClient::send_goal, this));
	}

	//定时器中断函数
	void send_goal()
	{
		using namespace std::placeholders;

		//取消了定时，也就是说定时器中断函数只执行一次
		this->timer_->cancel();

		//然后，看看有没有在线的同名动作服务器
		if (!this->client_ptr_->wait_for_action_server()) 
		{
			RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
			rclcpp::shutdown();
		}

		//定义目标
		auto goal_msg = Fibonacci::Goal();
		goal_msg.order = 10;

		RCLCPP_INFO(this->get_logger(), "Sending goal");

		//确定回调函数：请求后的响应、反馈的处理、结果的接收
		auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
		
		send_goal_options.goal_response_callback =
		std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
		
		send_goal_options.feedback_callback =
		std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
		
		send_goal_options.result_callback =
		std::bind(&FibonacciActionClient::result_callback, this, _1);
		
		//发送目标给动作服务器
		this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
	}

private:
	rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
	rclcpp::TimerBase::SharedPtr timer_;

	//请求后的响应
	void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
	{
		auto goal_handle = future.get();
		if (!goal_handle) 
		{
			RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
		} 
		else 
		{
			RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
		}
	}

	//反馈的处理
	void feedback_callback(GoalHandleFibonacci::SharedPtr, const std::shared_ptr<const Fibonacci::Feedback> feedback)
	{
		std::stringstream ss;
		ss << "Next number in sequence received: ";
		for (auto number : feedback->partial_sequence) 
		{
			ss << number << " ";
		}
		RCLCPP_INFO(this->get_logger(), ss.str().c_str());
	}

	//结果的接收
	void result_callback(const GoalHandleFibonacci::WrappedResult & result)
	{
		switch (result.code) 
		{
			case rclcpp_action::ResultCode::SUCCEEDED: 	break;
			case rclcpp_action::ResultCode::ABORTED: 	RCLCPP_ERROR(this->get_logger(), "Goal was aborted");	return;
			case rclcpp_action::ResultCode::CANCELED:	RCLCPP_ERROR(this->get_logger(), "Goal was canceled");	return;
			default:					RCLCPP_ERROR(this->get_logger(), "Unknown result code");	return;
		}
		std::stringstream ss;
		ss << "Result received: ";
		for (auto number : result.result->sequence) 
		{
			ss << number << " ";
		}
		RCLCPP_INFO(this->get_logger(), ss.str().c_str());
		rclcpp::shutdown();
	}
};  // class FibonacciActionClient

}  // namespace action_tutorials_cpp

RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient)

注 std::future ：看这里。std::thread是C++11中提供异步创建多线程的工具，但想要从线程中返回异步任务结果，一般需要依靠全局变量，从安全角度看，有些不妥，为此C++11提供了std::future类模板，future对象提供访问异步操作结果的机制，很轻松解决从异步任务中返回结果。

1.4 修改CMakelists.txt

在CMakelists.txt添加以下内容。

# 这里是生成共享库
add_library(action_server SHARED src/fibonacci_action_server.cpp)
add_library(action_client SHARED src/fibonacci_action_client.cpp)

target_include_directories(action_server PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_include_directories(action_client PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
  
target_compile_definitions(action_server PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
target_compile_definitions(action_client PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
  
ament_target_dependencies(action_server
  "action_tutorials_interfaces"
  "rclcpp"
  "rclcpp_action"
  "rclcpp_components")
ament_target_dependencies(action_client
  "action_tutorials_interfaces"
  "rclcpp"
  "rclcpp_action"
  "rclcpp_components")
  
rclcpp_components_register_node(action_server PLUGIN "action_tutorials_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server)
rclcpp_components_register_node(action_client PLUGIN "action_tutorials_cpp::FibonacciActionClient" EXECUTABLE fibonacci_action_client)

install(TARGETS
  action_server
  action_client
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)