一、在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