一、在ROS2环境下创建功能包

        如果您已经完成了上一小节的内容，那么接下来您一定渴望自己创建一个功能包来实现相应的功能。在ROS1中，您创建的功能包可以既写C/C++，又写python，但ROS2中不允许用户这么做，您的C/C++和python代码创建的功能包是不一样的。

二、创建功能包的流程

ros2 pkg create <package_name> --build-type ament_cmake 

ros2 pkg create <package_name> --build-type ament_python

这两句代码分别对应两种不同的功能包，我们创个C/C++的来看看怎么个事。

这里警告了，原因是咋们的许可证没选择在ROS2官方文档里面是带了这个参数项的

--license Apache-2.0

ros2 pkg create --build-type ament_cmake --license Apache-2.0 <package_name>

他的功能包名字放在最后，参数里除了提示了是C/C++语言还提到了许可证，但不填似乎问题不大，当然您不过看到警告就烦体质可以带上。方便复制下面。

ros2 pkg create <package_name> --build-type --license Apache-2.0 ament_cmake 

三、编写代码

        这次编写代码原本我是想用像大多数一样的class类继承rclcpp::Node的方式来写的，但是奈何技术不够，磕了几天感觉头疼换和ROS1类似的方式写了下面的代码，同时呢我想让本次的测试更有观赏性和参考性，我用了turtlesim功能包作为我的辅助，同时详细介绍下msg文件如何include。

a、首先我下载了turtlesim功能包

        您可以 ROS Package: turtlesim 先ros.index网站找到功能包，详细细节您可以看我之前的一篇文章《Ubuntu20.04环境下的ROS进阶学习0》_ubuntu下查看ros-CSDN博客

总而言之您只需要操作以下代码就可以下载该功能包：

cd ~/ros2_ws/src

git clone https://github.com/ros_tutorials.git

b、各种msg消息类型

        同样的操作搜索common_interfaces功能包，如下操作也行。

cd ~/ros2_ws/src

git clone https://github.com/ros2/common_interfaces.git

c、下载eigen3库

        这个和之前不一样，这个是一个C++的头文件集，用来处理向量、矩阵和线性代数相关的计算和转换。

cd ~/code        (没有记得建一个文件夹)

git clone https://gitlab.com/libeigen/eigen.git

git完后咱么那就要解压安装了。

cd eigen

mkdir build

cd build 

cmake ..

sudo make install

d、编写代码

 cd ~/ros2_ws/src

touch turtle_control.cpp 

        代码实现了turtle_control节点订阅turtle1/pose同时向turtle1/cmd_vel里面发送消息的功能。

所以在实际测试的时候得运行turtlesim节点。

#include <chrono>	//C++标准库，用于处理时间和日期的工具,特别是处理时间间隔和计数器
#include <memory>	//C++标准库，提供了智能指针，用于管理动态分配的内存，以避免内存泄漏
#include <string>	//C++标准库的头文件，提供了string类型的字符串相关操作函数
#include <eigen3/Eigen/Geometry> 	// C++ 用来提供向量和矩阵运算的头文件库

#include "rclcpp/rclcpp.hpp"          // ROS2 C++接口库
#include "std_msgs/msg/string.hpp"    // 字符串消息类型
#include "turtlesim/msg/pose.hpp"
#include "geometry_msgs/msg/twist.hpp"		//位置好像是 /ros2_ws/install/geometry_msgs/include/geometry_msgs/geometry_msgs/msg


class Turtle_Pose_Sub
{
	public:
		Turtle_Pose_Sub() : receive_flag_(false){}
		
		void Turtle_Pose_Callback(const turtlesim::msg::Pose::ConstPtr& msg)
		{
			turtle_pose_msgs_ = *msg;
			receive_flag_ = true;
		}
		
		bool Get_Turtle_Info(turtlesim::msg::Pose& msg)
		{
			if(receive_flag_)
			{
				msg = turtle_pose_msgs_;
				return true;
			}
			return false;
		}
	

	private:
		bool receive_flag_;
		turtlesim::msg::Pose turtle_pose_msgs_;
		
	
};

int main(int argc , char * argv[])
{
	rclcpp::init(argc , argv);	//初始化rclcpp
	rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("turtle_control");	//创建节点
	rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr turtle1_cmd_vel_pub = node->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel" , 10);
	
	Turtle_Pose_Sub Turtle_pose_sub_class;
	//rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr turtle1_pose_sub = node->create_subscription<turtlesim::msg::Pose>("/turtle1/pose" , 10 , &Turtle_Pose_Sub::Turtle_Pose_Callback , &Turtle_pose_sub_class);
	rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr turtle1_pose_sub = node->create_subscription<turtlesim::msg::Pose>("/turtle1/pose" , 10 , std::bind(&Turtle_Pose_Sub::Turtle_Pose_Callback , &Turtle_pose_sub_class , std::placeholders::_1));
	
	rclcpp::WallRate loop_rate(10);	// 创建时钟 并设置评率为1s 10次
	
	geometry_msgs::msg::Twist turtle1_cmd_vel_msg;
	turtlesim::msg::Pose turtle1_pose_msg;
	Eigen::Matrix<double, 2, 1> turtle_point;
	Eigen::Matrix<double, 2, 1> target_point;
	Eigen::Matrix<double, 2, 1> P;
	Eigen::Matrix<double, 2, 1> U;
	Eigen::Matrix<double, 2, 2> R_;
	double theta , distance;
	
	target_point << 6.0 , 7.0;
	
	std::cout << "i will go to while" << std::endl;
	while(rclcpp::ok())
	{
		rclcpp::spin_some(node);
		try
		{
			//std::cout << "i am trying it" << std::endl;
			if(Turtle_pose_sub_class.Get_Turtle_Info(turtle1_pose_msg))
			{
				//std::cout << turtle1_pose_msg.x << std::endl;
				turtle_point << turtle1_pose_msg.x , turtle1_pose_msg.y;
				theta = turtle1_pose_msg.theta;
			}
		}
		catch(const std::out_of_range& e)
		{
			continue;
		}
		
		R_ << cos(theta),sin(theta),-sin(theta),cos(theta);
		P << target_point(0) - turtle_point(0) , target_point(1) - turtle_point(1);
		distance = sqrt(P.transpose()*P);
		std::cout << "distance = " << distance << "\n" << std::endl;
		std::cout << "turtle_point = \n" << turtle_point << "\n" << std::endl;
		std::cout << "theta = " << theta << std::endl;
		U = R_ * P;
		turtle1_cmd_vel_msg.linear.x = U(0);
		turtle1_cmd_vel_msg.angular.z = U(1);
		
		//这个限幅写的好丑，我建议去掉{},但我不喜欢。
		if(turtle1_cmd_vel_msg.linear.x > 2)
		{
			turtle1_cmd_vel_msg.linear.x = 2;
		}
		if(turtle1_cmd_vel_msg.linear.x < -2)
		{
			turtle1_cmd_vel_msg.linear.x = -2;
		}
		if(turtle1_cmd_vel_msg.angular.z > 2)
		{
			turtle1_cmd_vel_msg.angular.z = 2;
		}
		if(turtle1_cmd_vel_msg.angular.z < -2)
		{
			turtle1_cmd_vel_msg.angular.z = -2;
		}
		
		if(distance < 0.3)
		{
			turtle1_cmd_vel_msg.linear.x = 0;
			turtle1_cmd_vel_msg.angular.z = 0;
		}
		turtle1_cmd_vel_pub->publish(turtle1_cmd_vel_msg);
		
		loop_rate.sleep();
	}
	
	
	return 0;
}

        值得注意的一点是，该代码并没有像官方示例一样使用继承节点的关系继承 : rclcpp_Node类，而是类似于C语言的方式面向过程写的，在之后的改进我将试着使用。

e、修改CMakeList.txt

        在ROS2中的CMakeList.txt 和ROS1中有一定的区别，而在本次的教程中您只需要修改以下部分。

首先本次程序需要一下功能包，其中参数分别是：功能包名 、 REQUIRED（必须，即缺少报错）。

find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)

将我们写的CPP文件编译成可执行文件，同时添加依赖项。

add_executable(turtle_control src/turtle_control.cpp)
ament_target_dependencies(turtle_control rclcpp std_msgs geometry_msgs turtlesim)

指定可执行文件的位置。

install(TARGETS

  turtle_control
    
    DESTINATION lib/${PROJECT_NAME})

整体如下所示，之后为了篇幅将不再贴出。

cmake_minimum_required(VERSION 3.8)
project(turtle_control)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)

add_executable(turtle_control src/turtle_control.cpp)
ament_target_dependencies(turtle_control rclcpp std_msgs geometry_msgs turtlesim)

#add_executable(turtle_control_useclass src/turtle_control_useclass.cpp)
#ament_target_dependencies(turtle_control_useclass rclcpp std_msgs geometry_msgs turtlesim)


install(TARGETS

  turtle_control
	#turtle_control_useclass
	
	DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

f、编译运行代码

cd ~/ros2_ws

colcon build        (运行这段代码的时间有时可达3分钟，编译真的超慢ROS2)

当然如果您已经编译过，您只需要编译您新写的代码就行

参数是--packages-select + 功能包名

source install/local_setup.bash  (添加到路径)

这里可以写到 .bashrc 里

source ~/ros2_ws/install/local_setup.sh

运行这里我们一共打开两个终端：

第一个终端输入:  ros2 run turtlesim turtlesim_node

第二个终端输入:  ros2 run turtle_control turtle_control

        可以看到，这里的终端里面的坐标和距离目标点的距离，我们代码里面设置的distance < 0.3则停，如果您设置太小，小海龟会在那一直打圈，这个和我们的控制律有关系，之后在解决这不是本次的重点。

四、参考

Creating a package — ROS 2 Documentation: Humble documentation

ROS2 发布/订阅Topic_ros2 发布订阅-CSDN博客

[Eigen中文文档] 从入门开始..._eigen库下载-CSDN博客