ROS2_Foxy学习（五）基础编程_C++

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

1 workspace

1.1 工作空间及其层次

  空间的创建：ROS2与ROS1一样，创建工作空间目录，并在其中创建/src目录，用于存放package功能包。

  层次的概念：ROS2增加了overlay和underlay的概念，用于在多个workspace同时工作时，处理各个workspace之间的层次问题，对于包名相同的package，上层workspace将覆盖(override)下层workspace中的同名package。

  层次的配置：层次是通过环境变量配置的先后决定的，ROS通过setup.bash设置环境变量，ROS2安装路径一般设置为最下层的工作空间，即

$ source /opt/ros/foxy/setup.bash

  运行新建workspace的package功能包，需要在对工作空间编译后的新终端中配置如下

# 设置当前工作空间中功能包的相关环境变量
$ . install/local_setup.sh 

# 设置当前工作空间中功能包的相关环境变量，同时设置该工作空间下其他底层工作空间的环境变量
$ . install/setup.sh	# 就是说这一个捎带着把ROS2安装路径的环境变量也设置了

1.2 检查依赖

  编译前，需要对功能包检查依赖情况。

$ rosdep install -i --from-path src --rosdistro foxy -y

1.3 编译工具 colcon

# 在workspace根目录编译工程
$ colcon build	

# 有选择地编译包
$ colcon build --packages-select 

  有关编译后面也会单列一小节~

2 编写 package

  每一个package都能实现一个相对完整的功能。

2.1 功能包创建

# 创建cmake功能包（在~/ws/src下）
$ ros2 pkg create --build-type ament_cmake 

# 创建cmake功能包（在~/ws/src下）
# --dependencies 参数会将后边的依赖自动添加到package.xml和CMakeLists.txt中
$ ros2 pkg create --build-type ament_cmake  --dependencies 

# 创建cmake功能包（在~/ws/src下）
# --node-name 参数将创建节点，但只能创建一个节点，注意参数位置
$ ros2 pkg create --build-type ament_cmake --node-name  

	# 例如（在~/ws/src下）
	$ ros2 pkg create --build-type ament_cmake --node-name my_node my_package --dependencies rclcpp
	# 将会在~/ws/src创建如下目录
	workspace_folder/
	    src/ 
	        my_package/ 
	        	 src/
	        	 	my_node.cpp
	        	 include/
	        	 	my_package/
	             CMakeLists.txt
	             package.xml

  package.xml：对功能包的描述。在自动创建后，需要手动修改功能包描述、功能包版本、功能包许可证信息等。以“ _depend”结尾的标签用来描述功能包依赖，在增加新的依赖时，需要添加到其中，方便查看。

    <description>这里写功能描述description>
    <maintainer email="这里写邮箱地址">Your Namemaintainer>
    <license>这里写许可证license>

  CMakeLists.txt：制定各文件或包的编译顺序的文档

2.2 功能包编译、设置环境变量、运行

# 记得检查依赖
$ rosdep install -i --from-path src --rosdistro foxy -y

# 编译，见上一小节
$ colcon build

# 新终端设置环境变量
$ . install/setup.bash

# 运行包里的某个节点，运行多个节点、多个包，可使用launch文件
$ ros2 run  

3 编写 topic

新建工作空间，并新建话题功能包（这里包名是cpp_pubsub）。

3.1 发布 publisher

在功能包的/src目录下，编写话题发布节点，publisher_member_function.cpp

#include 			//c++11日期和时间库		
#include 		//参见 注1
#include 			//c++内存管理库，使用智能指针必须包含此头文件
#include 

#include "rclcpp/rclcpp.hpp"	//ROS2，C++接口的头文件
#include "std_msgs/msg/string.hpp"	//string类型的msg头文件

using namespace std::chrono_literals;

/* This example creates a subclass of Node and uses std::bind() to register a
* member function as a callback from the timer. */

//定义一个节点类的子类
class MinimalPublisher : public rclcpp::Node
{
     
  public:
	  	//构造函数：初始化节点名 minimal_publisher，然后创建了一个publisher和定时器
	    MinimalPublisher(): Node("minimal_publisher"), count_(0)
	    {
     
	    	//创建一个发布者，发布的话题名为topic，话题消息是String，保存消息的队列长度是10
      		publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
	      	//创建一个定时器，定时500ms，定时触发回调函数timer_callback。
	      	//这里用到了的一个特性
	      	timer_ = this->create_wall_timer(500ms, std::bind(&MinimalPublisher::timer_callback, this));
	    }

  private:
  		//定时触发回调函数
	    void timer_callback()
	    {
     
	    	//定义消息
		    auto message = std_msgs::msg::String();
		    message.data = "Hello, world! " + std::to_string(count_++);
		    //打印日志
		    RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
		    //发布
		    publisher_->publish(message);
	    }
    
    	//定义定时器和发布者指针
	    rclcpp::TimerBase::SharedPtr timer_;
	    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
	    
	    size_t count_;
  };

  int main(int argc, char * argv[])
  {
     
	    rclcpp::init(argc, argv);
	    rclcpp::spin(std::make_shared<MinimalPublisher>());
	    rclcpp::shutdown();
	    return 0;
  }

注：
1、#include < functional > ，感兴趣的读者可以看下这篇博客，写的蛮不错的。

3.2 订阅 subscriber

在功能包的/src目录下，编写话题订阅节点，subscriber_member_function.cpp

#include 

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using std::placeholders::_1;

class MinimalSubscriber : public rclcpp::Node
{
     
public:
    MinimalSubscriber() : Node("minimal_subscriber")
    {
     
      subscription_ = this->create_subscription<std_msgs::msg::String>(
      "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
    }

private:
    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
    {
     
      RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
    }
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};

int main(int argc, char * argv[])
{
     
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalSubscriber>());
  rclcpp::shutdown();
  return 0;
}

3.3 修改 package.xml / CMakeLists.txt

  package.xml 添加如下依赖，如果创建包时，使用了–dependencies，则文件内已有如下语句。

    <depend>rclcppdepend>
    <depend>std_msgsdepend>

  CMakeLists.txt 添加如下编译规则，如果创建包时，使用了–dependencies，则文件内已有find_package相关的语句。

# 搜索依赖
    find_package(rclcpp REQUIRED)
    find_package(std_msgs REQUIRED)
# 创建可执行程序talker，后续参数是构成可执行文件的所有源文件
    add_executable(talker src/publisher_member_function.cpp)
# 链接find_package()找到的包
    ament_target_dependencies(talker rclcpp std_msgs)

    add_executable(listener src/subscriber_member_function.cpp)
    ament_target_dependencies(listener rclcpp std_msgs)

    install(TARGETS
      talker
      listener
      DESTINATION
      lib/${PROJECT_NAME})

注：ament_target_dependencies，这篇文章解释了与include_directories()和target_link_libraries()的区别与联系。

3.4 编译、运行两个节点

# 在工作空间根目录下，检查依赖
$ rosdep install -i --from-path src --rosdistro foxy -y
# 编译
$ colcon build

# 配置环境变量，然后运行
$ . install/setup.bash
$ ros2 run cpp_pubsub talker

# 打开另一个终端，配置环境变量，运行
$ . install/setup.bash
$ ros2 run cpp_pubsub listener

4 编写 service

新建工作空间，并新建服务功能包（这里包名是cpp_srvcli）。

4.1 服务器端 server

在功能包的/src目录下，创建服务器端节点，add_two_ints_server.cpp

#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"
#include 

void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
      std::shared_ptr<example_interfaces::srv::AddTwoInts::Response>      response)
{
     
	response->sum = request->a + request->b;
	RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",
		    request->a, request->b);
	RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}

int main(int argc, char **argv)
{
     
	rclcpp::init(argc, argv);

	//创建名为add_two_ints_server的节点
	std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");


	//创建名为add_two_ints的服务，绑定回调函数add
	rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service =
	node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);


	RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");

	//入锁，等待客户端请求
	rclcpp::spin(node);
	rclcpp::shutdown();
}

4.2 客户端 client

在功能包的/src目录下，创建客户端节点，add_two_ints_client.cpp

#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"

#include 
#include 
#include 

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
     
	rclcpp::init(argc, argv);

	if (argc != 3) 
	{
     
		RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_two_ints_client X Y");
		return 1;
	}

	//创建名为add_two_ints_client的节点
	std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_client");

	//创建名为add_two_ints的客户端
	rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client =
	node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");

	//采集request数据
	auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
	request->a = atoll(argv[1]);
	request->b = atoll(argv[2]);

	//search for service
	while (!client->wait_for_service(1s)) 
	{
     
		if (!rclcpp::ok()) 
		{
     
			RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
			return 0;
		}
		RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
	}

	//发送request
	auto result = client->async_send_request(request);
	
	// Wait for the result.
	if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)
	{
     
		RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);
	} 
	else 
	{
     
		RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints");
	}

	rclcpp::shutdown();
	return 0;
}

4.3 修改 package.xml / CMakeLists.txt

  package.xml 添加如下依赖，如果创建包时，使用了–dependencies，则文件内已有如下语句。

    <depend>rclcppdepend>
  	<depend>example_interfacesdepend>

  CMakeLists.txt 添加如下编译规则，如果创建包时，使用了–dependencies，则文件内已有find_package相关的语句。

# 搜索依赖
    find_package(rclcpp REQUIRED)
	find_package(example_interfaces REQUIRED)
# 创建可执行程序server，后续参数是构成可执行文件的所有源文件
    add_executable(server src/add_two_ints_server.cpp)
# 链接find_package()找到的包
    ament_target_dependencies(server rclcpp example_interfaces)

    add_executable(client src/add_two_ints_client.cpp)
	ament_target_dependencies(client rclcpp example_interfaces)

    install(TARGETS
			  server
			  client
			  DESTINATION lib/${PROJECT_NAME})

4.4 编译、运行两个节点

# 在工作空间根目录下，检查依赖
$ rosdep install -i --from-path src --rosdistro foxy -y
# 编译
$ colcon build

# 配置环境变量，然后运行
$ . install/setup.bash
$ ros2 run cpp_srvcli server

# 打开另一个终端，配置环境变量，运行
$ . install/setup.bash
$ ros2 run cpp_srvcli client 2 3

5 自定义消息 msg/srv

5.1 创建消息功能包

1、新建工作空间，并新建消息功能包（这里包名是tutorial_interfaces）。在消息功能包下，创建/msg和/srv文件夹（与/src同级），并在其中分别建立.msg和.srv文件，有关消息内容，详见本系列第三篇《ROS2_Foxy学习（三）核心概念》。

Num.msg

int64 num

AddThreeInts.srv

int64 a
int64 b
int64 c
---
int64 sum

2、在package.xml文件中添加

	<build_depend>rosidl_default_generatorsbuild_depend>
	<exec_depend>rosidl_default_runtimeexec_depend>
	<member_of_group>rosidl_interface_packagesmember_of_group>

注：rosidl_default_generators为编译依赖，rosidl_default_runtime为运行依赖。
3、在CMakeLists.txt文件中添加

find_package(rosidl_default_generators REQUIRED)

# You can use set to neatly list all of your interfaces:
set(msg_files
  "msg/Num.msg"
  "msg/Message1.msg"
  "msg/Message2.msg"
  # etc
  )

set(srv_files
  "srv/AddThreeInts.srv"
  "srv/Service1.srv"
  "srv/Service2.srv"
   # etc
  )

#And generate all lists at once like so:
rosidl_generate_interfaces(${PROJECT_NAME}
  ${msg_files}
  ${srv_files}
)

ament_export_dependencies(rosidl_default_runtime)

注：可以省略set的步骤，在rosidl_generate_interfaces直接添加，如下，但不建议，还是需要一个良好的编程习惯。

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Num.msg"
  "srv/AddThreeInts.srv"
)

5.2 测试自定义消息接口

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

5.3 应用自定义消息接口

  在同一个工作空间下，除消息功能包外，再创建两个功能包，分别是话题功能包和服务功能包，参考3、4部分。

1、CMakeLists.txt和package.xml与第3、4部分基本一致，其中，需要将消息依赖包（std_msgs和example_interfaces）替换成新建的消息功能包名，如tutorial_interfaces。
2、各节点src文件，需要将消息类型进行替换，src文件修改见附件1。
3、之后，编译运行（查依赖、编译包、配置环境、运行节点）。

5.4 使用同一功能包的消息接口

  若功能包需要使用自己定义的接口，需要在CMakeLists.txt中添加以下内容（In order to use the messages generated in the same package.）。

rosidl_target_interfaces(publish_address_book
  ${PROJECT_NAME} "rosidl_typesupport_cpp")

注：publish_address_book是编译生成的可执行程序名。

6 使用 parameter

新建工作空间，并新建参数功能包（这里包名是cpp_parameters），使用参数dependencies，自动添加依赖rclcpp。

6.1 参数的声明与获取

在功能包的/src目录下，创建节点，cpp_parameters_node.cpp

#include 
#include 
#include 
#include 

using namespace std::chrono_literals;

class ParametersClass: public rclcpp::Node
{
     
  public:
    ParametersClass() : Node("parameter_node")
    {
     
      //声明：定义参数名及其值
      this->declare_parameter<std::string>("my_parameter", "world");
      //
      timer_ = this->create_wall_timer(1000ms, std::bind(&ParametersClass::respond, this));
    }
    
    //获取：定时打印参数值
    void respond()
    {
     
      this->get_parameter("my_parameter", parameter_string_);
      RCLCPP_INFO(this->get_logger(), "Hello %s", parameter_string_.c_str());
    }
    
  private:
    std::string parameter_string_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char** argv)
{
     
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ParametersClass>());
  rclcpp::shutdown();
  return 0;
}

修改CMakelists.txt，然后编译运行。

add_executable(parameter_node src/cpp_parameters_node.cpp)
ament_target_dependencies(parameter_node rclcpp)

install(TARGETS
  parameter_node
  DESTINATION lib/${PROJECT_NAME}
)

6.2 终端修改参数

有关内容，详见本系列第三篇《ROS2_Foxy学习（三）核心概念》

$ ros2 param list
$ ros2 param set   

6.3 launch文件修改参数

在功能包的/launch目录下，创建launch文件，cpp_parameters_launch.py，修改parameters。

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package="cpp_parameters",
            executable="parameter_node",
            name="custom_parameter_node",
            output="screen",
            emulate_tty=True,
            parameters=[
                {
     "my_parameter": "earth"}
            ]
        )
    ])

修改CMakelists.txt，在6.1部分中CMakelists.txt最后增加以下部分。

install(
  DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

编译，然后运行launch文件。

$ ros2 launch cpp_parameters cpp_parameters_launch.py

附件1 消息文件应用

1、话题功能包——消息发布节点——publisher_member_function.cpp

#include 
#include 
#include 
#include 

#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/msg/num.hpp"

using namespace std::chrono_literals;

/* This example creates a subclass of Node and uses std::bind() to register a* member function as a callback from the timer. */

class MinimalPublisher : public rclcpp::Node
{
     
public:
	MinimalPublisher(): Node("minimal_publisher"), count_(0)
	{
     
	  publisher_ = this->create_publisher<tutorial_interfaces::msg::Num>("topic", 10); //10 是消息队列长度
	  timer_ = this->create_wall_timer(500ms,std::bind(&MinimalPublisher::timer_callback, this));
	}

private:
	void timer_callback()
	{
     
	  auto message = tutorial_interfaces::msg::Num();
	  message.num = this->count_++;
	  RCLCPP_INFO(this->get_logger(), "Publishing: '%d'", message.num);
	  publisher_->publish(message);
	}
	
	rclcpp::TimerBase::SharedPtr timer_;
	
	rclcpp::Publisher<tutorial_interfaces::msg::Num>::SharedPtr publisher_;
	
	size_t count_;
};

int main(int argc, char * argv[])
{
     
	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<MinimalPublisher>());
	rclcpp::shutdown();
	return 0;
}

2、话题功能包——消息订阅节点——subscriber_member_function.cpp

#include 
#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/msg/num.hpp"

using std::placeholders::_1;

class MinimalSubscriber : public rclcpp::Node
{
     
public:
	MinimalSubscriber(): Node("minimal_subscriber")
	{
     
	  subscription_ = this->create_subscription<tutorial_interfaces::msg::Num>("topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
	}

private:
	void topic_callback(const tutorial_interfaces::msg::Num::SharedPtr msg) const
	{
     
	  RCLCPP_INFO(this->get_logger(), "I heard: '%d'", msg->num);
	}
	rclcpp::Subscription<tutorial_interfaces::msg::Num>::SharedPtr subscription_;};

int main(int argc, char * argv[])
{
     
	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<MinimalSubscriber>());
	rclcpp::shutdown();
	return 0;
}

3、服务功能包——服务器端节点——add_three_ints_server.cpp

#include "tutorial_interfaces/srv/add_three_ints.hpp"
#include "rclcpp/rclcpp.hpp"
#include 

void add(const std::shared_ptr<tutorial_interfaces::srv::AddThreeInts::Request> request,
      std::shared_ptr<tutorial_interfaces::srv::AddThreeInts::Response>      response)
{
     
	response->sum = request->a + request->b + request->c;
	RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld" " c: %ld",
		    request->a, request->b, request->b);
	RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}

int main(int argc, char **argv)
{
     
	rclcpp::init(argc, argv);

	//创建名为add_two_ints_server的节点
	std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_three_ints_server");


	//创建名为add_two_ints的服务，绑定回调函数add
	rclcpp::Service<tutorial_interfaces::srv::AddThreeInts>::SharedPtr service =
	node->create_service<tutorial_interfaces::srv::AddThreeInts>("add_three_ints", &add);


	RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add three ints.");

	//入锁，等待客户端请求
	rclcpp::spin(node);
	rclcpp::shutdown();
}

4、服务功能包——客户端节点——add_three_ints_client.cpp

#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/srv/add_three_ints.hpp"

#include 
#include 
#include 

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
     
	rclcpp::init(argc, argv);

	if (argc != 4) 
	{
     
		RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_two_ints_client X Y Z");
		return 1;
	}

	//创建名为add_three_ints_client的节点
	std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_three_ints_client");

	//创建名为add_three_ints的客户端
	rclcpp::Client<tutorial_interfaces::srv::AddThreeInts>::SharedPtr client =
	node->create_client<tutorial_interfaces::srv::AddThreeInts>("add_three_ints");

	//采集request数据
	auto request = std::make_shared<tutorial_interfaces::srv::AddThreeInts::Request>();
	request->a = atoll(argv[1]);
	request->b = atoll(argv[2]);
	request->c = atoll(argv[3]);

	//search for service
	while (!client->wait_for_service(1s)) 
	{
     
		if (!rclcpp::ok()) 
		{
     
			RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
			return 0;
		}
		RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
	}

	//发送request
	auto result = client->async_send_request(request);
	
	// Wait for the result.
	if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)
	{
     
		RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);
	} 
	else 
	{
     
		RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_three_ints");
	}

	rclcpp::shutdown();
	return 0;
}