创建包

1# ament_cmake,默认
2ros2 pkg create --build-type ament_cmake <package_name>
3# cmake
4ros2 pkg create --build-type cmake <package_name>

迁移包

1<depend>foo</depend>
2<!-- <depend>等于以下 -->
3<build_depend>foo</build_depend>
4<build_export_depend>foo</build_export_depend>
5<exec_depend>foo</exec_depend>
6<!-- ros2没有以下 -->
7<run_depend>foo</run_depend>

迁移接口

消息,服务和动作在ROS统称接口(interfaces)

package依赖添加

1<buildtool_depend>rosidl_default_generators</buildtool_depend>
2<exec_depend>rosidl_default_runtime</exec_depend>
3<member_of_group>rosidl_interface_packages</member_of_group>
4<depend>message_package</depend>

CMake添加C++17支持

1set(CMAKE_CXX_STANDARD 17)
2find_package(rosidl_default_generators REQUIRED)
3find_package(message_package REQUIRED)
4rosidl_generate_interfaces(${PROJECT_NAME}
5  "msg/Num.msg"
6  "msg/Sphere.msg"
7  "srv/AddThreeInts.srv"
8  DEPENDENCIES geometry_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg
9)

自定义消息文档: https://docs.ros.org/en/rolling/Tutorials/Beginner-Client-Libraries/Custom-ROS2-Interfaces.html

迁移C++包

构建工具

使用colcon,教程: https://docs.ros.org/en/rolling/Tutorials/Beginner-Client-Libraries/Colcon-Tutorial.html

构建系统

ROS2 使用ament构建

package.xml里面加上

1<buildtool_depend>ament_cmake</buildtool_depend>
2<!-- your dependencies -->
3<export>
4  <build_type>ament_cmake</build_type>
5</export>

找包方式

1find_package(ament_cmake REQUIRED)
2find_package(component1 REQUIRED)
3# ...
4find_package(componentN REQUIRED)

在所有包注册好后调用ament_package

catkin_package里面的CATKIN_DEPENDSINCLUDE_DIRSLIBRARIES改为使用三条cmake函数ament_export_dependenciesament_export_include_directoriesament_export_libraries,这几条命令需要在ament_package之前调用

替换add_message_files, add_service_filesgenerate_messagesrosidl_generate_interfaces

1rosidl_generate_interfaces(${PROJECT_NAME}
2  ${msg_files}
3  DEPENDENCIES std_msgs
4)

单元测试

检查器(Linters)

更新源码

消息,服务和动作

ROS2的消息,服务和动作都在不同的命名空间下,例如#include <my_interfaces/msg/my_message.hpp>my_interfaces::msg::MyMessage

共享指针的类型也变了:my_interfaces::msg::MyMessage::SharedPtr

更详细的文档: generated C++ interfaces

使用服务

ROS2服务没有返回值了

 1// ROS 1 style is in comments, ROS 2 follows, uncommented.
 2// #include "nav_msgs/GetMap.h"
 3#include "nav_msgs/srv/get_map.hpp"
 4
 5// bool service_callback(
 6//   nav_msgs::GetMap::Request & request,
 7//   nav_msgs::GetMap::Response & response)
 8void service_callback(
 9  const std::shared_ptr<nav_msgs::srv::GetMap::Request> request,
10  std::shared_ptr<nav_msgs::srv::GetMap::Response> response)
11{
12  // ...
13  // return true;  // or false for failure
14}

ros::Time迁移

  • 把所有的ros::Time改为rclcpp::Time
  • 如果代码中有用到std_msgs::Time:
    • 把所有td_msgs::Time实例改为builtin_interfaces::msg::Time
    • 把所有包含#include "std_msgs/time.h改为#include "builtin_interfaces/msg/time.hpp"
    • 把所有std_msgs::Time中的nsec改为builtin_interfaces::msg::Timenanosec
  • ros::Time::now()改为node->now()
1namespace rclcpp {
2  ...
3  explicit Clock(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
4}

[!NOTE] ROS2中不同时钟类型(clock_type)的时间(rclcpp::Time)类型是不能进行运算比较等操作的。rclcpp::Clock().now()返回的是系统时间, 加参数RCL_ROS_TIME后等同ros::Time::now(),默认是系统时间,也就是WallTime

ROS2中rclcpp::Time如果没初始化就做比较会崩溃,报错,和上面的问题是一样的,不像ROS1,默认用0给ros::Time赋值

terminate called after throwing an instance of 'std::runtime_error'
  what():  can't subtract times with different time sources [2 != 1]

用以下代码检查报文是否初始化时间戳

1auto locateTime = rclcpp::Time(locate.header.stamp);
2if (locateTime.get_clock_type() == RCL_CLOCK_UNINITIALIZED) {
3    LOGW_HZ(0.5, "定位时间戳类型错误");
4}

ros::Rate迁移

ros::Rate改为rclcpp::Rate

Boost

不受影响,注意版本接口就行,但是ROS2自带的函数和智能指针全用std标准库的了

线程和锁

  • boost::mutex::scoped_lock -> std::unique_lock<std::mutex>
  • boost::mutex -> std::mutex
  • #include <boost/thread/mutex.hpp> -> #include <mutex>

无序map

boost改为标准c++的

function

boost改为标准c++的

实际迁移例子

https://docs.ros.org/en/rolling/How-To-Guides/Migrating-from-ROS1/Migrating-CPP-Packages.html#example-converting-an-existing-ros-1-package-to-ros-2

迁移Python包

迁移launch文件

设计文档: https://docs.ros.org/en/rolling/Tutorials/Intermediate/Launch/Launch-system.html

例子: http://design.ros2.org/articles/roslaunch_xml.html

迁移标签

launch

ROS2中根元素为launch

node

  • type -> exec
  • ns -> namespace
  • 移除machine, respawn_delay, clear_params.

param

  • ROS2没有全局参数,不支持的标签有type, textfile, binfile, executable
  • command属性变为value="$(command '...' )"

类型规则

例子

 1<node pkg="my_package" exec="my_executable" name="my_node">
 2   <!--A string parameter with value "1"-->
 3   <param name="a_string" value="'1'"/>
 4   <!--A integer parameter with value 1-->
 5   <param name="an_int" value="1"/>
 6   <!--A float parameter with value 1.0-->
 7   <param name="a_float" value="1.0"/>
 8   <!--A string parameter with value "asd"-->
 9   <param name="another_string" value="asd"/>
10   <!--Another string parameter, with value "asd"-->
11   <param name="string_with_same_value_as_above" value="'asd'"/>
12   <!--Another string parameter, with value "'asd'"-->
13   <param name="quoted_string" value="\'asd\'"/>
14   <!--A list of strings, with value ["asd", "bsd", "csd"]-->
15   <param name="list_of_strings" value="asd, bsd, csd" value-sep=", "/>
16   <!--A list of ints, with value [1, 2, 3]-->
17   <param name="list_of_ints" value="1,2,3" value-sep=","/>
18   <!--Another list of strings, with value ["1", "2", "3"]-->
19   <param name="another_list_of_strings" value="'1';'2';'3'" value-sep=";"/>
20   <!--A list of strings using an strange separator, with value ["1", "2", "3"]-->
21   <param name="strange_separator" value="'1'//'2'//'3'" value-sep="//"/>
22</node>

参数组

1<node pkg="my_package" exec="my_executable" name="my_node" ns="/an_absoulute_ns">
2   <param name="group1">
3      <param name="group2">
4         <param name="my_param" value="1"/>
5      </param>
6      <param name="another_param" value="2"/>
7   </param>
8</node>

rosparam

从其他文件中加载配置,改为from

1<node pkg="my_package" exec="my_executable" name="my_node" ns="/an_absoulute_ns">
2   <param from="/path/to/file"/>
3</node>

remap

只准在node中使用

include

https://docs.ros.org/en/rolling/How-To-Guides/Migrating-from-ROS1/Migrating-Launch-Files.html#replacing-an-include-tag

arg

  • value标签不允许,使用let标签
  • doc变为description
  • include标签里面时,ifunlessdescription都不允许

tf到tf2静态变换迁移

1<node pkg="tf2" exec="static_transform_publisher" name="vehicleTransPublisher"
2          args="-$(arg sensorOffsetX) -$(arg sensorOffsetY) 0 0 0 0 /sensor /vehicle 1000"/>
3
4<node pkg="tf2_ros" exec="static_transform_publisher" name="vehicleTransPublisher" args="-$(var sensorOffsetX) -$(var sensorOffsetY) 0 0 0 0 /sensor /vehicle"/>

迁移参数

迁移脚本

自动转换工具

Magical ROS 2 Conversion Tool

启动文件迁移器,用于将ROS 1的XML启动文件转换为ROS 2的Python启动文件:https://github.com/aws-robotics/ros2-launch-file-migrator

亚马逊已经提供了他们用于从ROS 1迁移到ROS 2的工具,链接如下:https://github.com/awslabs/ros2-migration-tools/tree/master/porting_tools

rospy2 Python项目,用于自动将rospy调用转换为rclpy调用

其他迁移技巧

ROS命令行

  • rosrun -> ros2 run
  • rosparam -> ros2 param
  • 其他类似

ROS命令行参数

ROS2命令行参数必须在--ros-args之后

1ros2 run some_package some_ros_executable --ros-args -r foo:=bar

参数

1ros2 run some_package some_ros_executable --ros-args -p my_param:=value

更改节点名

1ros2 run some_package some_ros_executable --ros-args -r __node:=new_node_name

更改命名空间

1ros2 run some_package some_ros_executable --ros-args -r __ns:=/new/namespace

ROS发布订阅

 1#include <rclcpp/rclcpp.hpp>
 2#include <std_msgs/msg/string.hpp>
 3#include <sensor_msgs/msg/point_cloud2.hpp>
 4
 5class MyNode : public rclcpp::Node
 6{
 7public:
 8  MyNode() : Node("my_node")
 9    {
10      publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
11      subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
12      "topic", rclcpp::SensorDataQoS(), [this] (sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { topic_callback(msg); });
13    }
14private:
15  void topic_callback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) const
16    {
17      RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
18    }
19private:
20  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
21  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
22}
23
24int main(int argc, char * argv[])
25{
26  rclcpp::init(argc, argv);
27  rclcpp::spin(std::make_shared<MyNode>());
28  rclcpp::shutdown();
29  return 0;
30}

使用lambda表达式可以避免使用std::bind绑定订阅回调函数时出现以下错误(不一定出现):

  113 |     const_shared_ptr_callback_ = callback;
      |     ~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~
In file included from /usr/include/c++/9/future:48,
                 from /opt/ros/foxy/include/rclcpp/executors.hpp:18,
                 from /opt/ros/foxy/include/rclcpp/rclcpp.hpp:146,
                 from /home/jiang/code/general_robot/src/locate_3d/src/LidarLocate.hpp:5,
                 from /home/jiang/code/general_robot/src/locate_3d/src/LidarLocate.cpp:1:
/usr/include/c++/9/bits/std_function.h:462:7: note: candidate: ‘std::function<_Res(_ArgTypes ...)>& std::function<_Res(_ArgTypes ...)>::operator=(const std::function<_Res(_ArgTypes ...)>&) [with _Res = void; _ArgTypes = {std::shared_ptr<const geometry_msgs::msg::PoseStamped_<std::allocator<void> > >}]’
  462 |       operator=(const function& __x)
      |       ^~~~~~~~
/usr/include/c++/9/bits/std_function.h:462:33: note:   no known conversion for argument 1 from ‘std::_Bind<void (LidarLocate::*(LidarLocate*, boost::arg<1>))(std::shared_ptr<const geometry_msgs::msg::PoseStamped_<std::allocator<void> > >)>’ to ‘const std::function<void(std::shared_ptr<const geometry_msgs::msg::PoseStamped_<std::allocator<void> > >)>&’
  462 |       operator=(const function& __x)
      |                 ~~~~~~~~~~~~~~~~^~~
/usr/include/c++/9/bits/std_function.h:480:7: note: candidate: ‘std::function<_Res(_ArgTypes ...)>& std::function<_Res(_ArgTypes ...)>::operator=(std::function<_Res(_ArgTypes ...)>&&) [with _Res = void; _ArgTypes = {std::shared_ptr<const geometry_msgs::msg::PoseStamped_<std::allocator<void> > >}]’
  480 |       operator=(function&& __x) noexcept
      |       ^~~~~~~~
/usr/include/c++/9/bits/std_function.h:480:28: note:   no known conversion for argument 1 from ‘std::_Bind<void (LidarLocate::*(LidarLocate*, boost::arg<1>))(std::shared_ptr<const geometry_msgs::msg::PoseStamped_<std::allocator<void> > >)>’ to ‘std::function<void(std::shared_ptr<const geometry_msgs::msg::PoseStamped_<std::allocator<void> > >)>&&’
  480 |       operator=(function&& __x) noexcept
      |                 ~~~~~~~~~~~^~~
/usr/include/c++/9/bits/std_function.h:494:7: note: candidate: ‘std::function<_Res(_ArgTypes ...)>& std::function<_Res(_ArgTypes ...)>::operator=(std::nullptr_t) [with _Res = void; _ArgTypes = {std::shared_ptr<const geometry_msgs::msg::PoseStamped_<std::allocator<void> > >}; std::nullptr_t = std::nullptr_t]’
  494 |       operator=(nullptr_t) noexcept
      |       ^~~~~~~~
/usr/include/c++/9/bits/std_function.h:494:17: note:   no known conversion for argument 1 from ‘std::_Bind<void (LidarLocate::*(LidarLocate*, boost::arg<1>))(std::shared_ptr<const geometry_msgs::msg::PoseStamped_<std::allocator<void> > >)>’ to ‘std::nullptr_t’
  494 |       operator=(nullptr_t) noexcept
      |                 ^~~~~~~~~
/usr/include/c++/9/bits/std_function.h:523:2: note: candidate: ‘template<class _Functor> std::function<_Res(_ArgTypes ...)>::_Requires<std::function<_Res(_ArgTypes ...)>::_Callable<typename std::decay<_Functor>::type>, std::function<_Res(_ArgTypes ...)>&> std::function<_Res(_ArgTypes ...)>::operator=(_Functor&&) [with _Functor = _Functor; _Res = void; _ArgTypes = {std::shared_ptr<const geometry_msgs::msg::PoseStamped_<std::allocator<void> > >}]’
  523 |  operator=(_Functor&& __f)
      |  ^~~~~~~~

代码的C++标准为17

另外

rclcpp::Subscriptiontake函数:

rclcpp::Subscriptiontake函数用于从订阅的主题中取出一条消息。这个函数会尝试从订阅的主题中取出一条消息,并将其放入提供的消息实例中。如果成功取出一条消息,函数会返回true,否则返回false

这是take函数的原型:

1bool take(MessageT & message_out, rmw_message_info_t & message_info_out);

其中,MessageT是消息的类型,message_out是一个消息实例的引用,用于存储取出的消息。message_info_out是一个rmw_message_info_t实例的引用,用于存储关于取出的消息的元信息,如发布者的信息。==不确定能不能自己用==

动作的设计文档:http://design.ros2.org/articles/actions.html

动作客户端例子

 1#include <rclcpp/rclcpp.hpp>
 2#include <rclcpp_action/rclcpp_action.hpp>
 3#include <your_action_pkg/action/YourAction.hpp>  // 替换为你的动作消息头文件
 4
 5class ActionClientNode : public rclcpp::Node
 6{
 7public:
 8  ActionClientNode() : Node("action_client_node")
 9  {
10    this->client_ptr_ = rclcpp_action::create_client<your_action_pkg::action::YourAction>(this,
11      "your_action_name");  // 替换为你的动作名称
12
13    // 创建一个目标
14    auto goal_msg = your_action_pkg::action::YourAction::Goal();
15    // ... 填充目标消息
16
17    // 发送目标,并绑定结果回调函数
18    this->client_ptr_->async_send_goal(goal_msg, std::bind(&ActionClientNode::goal_response_callback, this, std::placeholders::_1));
19  }
20
21private:
22  void goal_response_callback( rclcpp_action::ClientGoalHandle<your_action_pkg::action::YourAction>::SharedPtr goal_handle)
23  {
24    if (!goal_handle) {
25      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
26    } else {
27      RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
28      goal_handle->async_result(std::bind(&ActionClientNode::result_callback, this, std::placeholders::_1));
29    }
30  }
31
32  void feedback_callback( rclcpp_action::ClientGoalHandle<your_action_pkg::action::YourAction>::SharedPtr goal_handle,
33    const std::shared_ptr<const your_action_pkg::action::YourAction::Feedback> feedback)
34  {
35    
36  }
37
38  void result_callback(const rclcpp_action::ClientGoalHandle<your_action_pkg::action::YourAction>::WrappedResult & result)
39  {
40    switch(result.code) {
41      case rclcpp_action::ResultCode::SUCCEEDED:
42        // 处理成功的结果
43        break;
44      case rclcpp_action::ResultCode::ABORTED:
45        // 处理被服务器中止的结果
46        break;
47      case rclcpp_action::ResultCode::CANCELED:
48        // 处理被客户端取消的结果
49        break;
50      default:
51        // 处理其他结果
52        break;
53    }
54  }
55
56  rclcpp_action::Client<your_action_pkg::action::YourAction>::SharedPtr client_ptr_;
57};
58
59int main(int argc, char **argv)
60{
61  rclcpp::init(argc, argv);
62  auto node = std::make_shared<ActionClientNode>();
63  rclcpp::spin(node);
64  rclcpp::shutdown();
65  return 0;
66}

动作服务端例子

 1#include <rclcpp/rclcpp.hpp>
 2#include <rclcpp_action/rclcpp_action.hpp>
 3#include <your_action_pkg/action/YourAction.hpp>  // 替换为你的动作消息头文件
 4
 5class ActionServerNode : public rclcpp::Node
 6{
 7public:
 8  ActionServerNode() : Node("action_server_node")
 9  {
10    using GoalHandle = rclcpp_action::ServerGoalHandle<your_action_pkg::action::YourAction>;
11
12    this->server_ptr_ = rclcpp_action::create_server<your_action_pkg::action::YourAction>(this,
13      "your_action_name",  // 替换为你的动作名称
14      std::bind(&ActionServerNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
15      std::bind(&ActionServerNode::handle_cancel, this, std::placeholders::_1),
16      std::bind(&ActionServerNode::handle_accepted, this, std::placeholders::_1));
17  }
18
19private:
20  rclcpp_action::GoalResponse handle_goal(
21    const rclcpp_action::GoalUUID & uuid,
22    your_action_pkg::action::YourAction::Goal::ConstSharedPtr goal)
23  {
24    // 处理新的目标请求
25    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
26  }
27
28  rclcpp_action::CancelResponse handle_cancel(
29    std::shared_ptr<rclcpp_action::ServerGoalHandle<your_action_pkg::action::YourAction>> goal_handle)
30  {
31    // 处理取消请求
32    return rclcpp_action::CancelResponse::ACCEPT;
33  }
34
35  void handle_accepted( std::shared_ptr<rclcpp_action::ServerGoalHandle<your_action_pkg::action::YourAction>> goal_handle)
36  {
37    // 处理已接受的目标
38  }
39
40  rclcpp_action::Server<your_action_pkg::action::YourAction>::SharedPtr server_ptr_;
41};
42
43int main(int argc, char **argv)
44{
45  rclcpp::init(argc, argv);
46  auto node = std::make_shared<ActionServerNode>();
47  rclcpp::spin(node);
48  rclcpp::shutdown();
49  return 0;
50}

服务客户端例子

 1#include <rclcpp/rclcpp.hpp>
 2#include <your_srv_pkg/srv/YourService.hpp>  // 替换为你的服务消息头文件
 3
 4class ServiceClientNode : public rclcpp::Node
 5{
 6public:
 7  ServiceClientNode() : Node("service_client_node")
 8  {
 9    this->client_ptr_ = this->create_client<your_srv_pkg::srv::YourService>("your_service_name");  // 替换为你的服务名称
10
11    // 创建一个服务请求
12    auto request = std::make_shared<your_srv_pkg::srv::YourService::Request>();
13    // ... 填充请求消息
14
15    // 发送服务请求,并绑定响应回调函数
16    this->client_ptr_->async_send_request(request, std::bind(&ServiceClientNode::response_callback, this, std::placeholders::_1));
17  }
18
19private:
20  void response_callback(const typename your_srv_pkg::srv::YourService::Response::SharedPtr response)
21  {
22    // 处理服务响应
23  }
24
25  rclcpp::Client<your_srv_pkg::srv::YourService>::SharedPtr client_ptr_;
26};
27
28int main(int argc, char **argv)
29{
30  rclcpp::init(argc, argv);
31  auto node = std::make_shared<ServiceClientNode>();
32  rclcpp::spin(node);
33  rclcpp::shutdown();
34  return 0;
35}

服务服务端例子

 1#include <rclcpp/rclcpp.hpp>
 2#include <your_srv_pkg/srv/YourService.hpp>  // 替换为你的服务消息头文件
 3
 4class ServiceServerNode : public rclcpp::Node
 5{
 6public:
 7  ServiceServerNode() : Node("service_server_node")
 8  {
 9    this->server_ptr_ = this->create_service<your_srv_pkg::srv::YourService>(
10      "your_service_name",  // 替换为你的服务名称
11      std::bind(&ServiceServerNode::handle_service, this, std::placeholders::_1, std::placeholders::_2));
12  }
13
14private:
15  void handle_service(
16    const your_srv_pkg::srv::YourService::Request::SharedPtr request,
17    your_srv_pkg::srv::YourService::Response::SharedPtr response)
18  {
19    // 处理服务请求,并填充响应消息
20  }
21
22  rclcpp::Service<your_srv_pkg::srv::YourService>::SharedPtr server_ptr_;
23};
24
25int main(int argc, char **argv)
26{
27  rclcpp::init(argc, argv);
28  auto node = std::make_shared<ServiceServerNode>();
29  rclcpp::spin(node);
30  rclcpp::shutdown();
31  return 0;
32}

函数替换:

  • getNumSubscribers() -> get_subscription_count()

跳过包编译

ROS1中在包根目录touch CATKIN_IGNORE,ROS2则是touch COLCON_IGNORE

消息修改

命名规则

  • 消息字段为xxxx_xxxx命名风格
  • 消息常量为XXXX_XXXX命名风格

常用消息迁移

ROS1类型 ROS2类型 匹配正则表达式
Header std_msgs/Header ^\s*Header
time builtin_interfaces/Time ^\s*time

单独编译包

单独编译某个包并把cmake输出打印到命令行

1colcon build --event-handlers console_direct+ --packages-select foo_package

CMake打印所有当前环境变量

1get_cmake_property(_variableNames VARIABLES)
2foreach (_variableName ${_variableNames})
3    message(STATUS "${_variableName}=${${_variableName}}")
4endforeach()

PCL

ROS2中没有pcl_ros包了,需要

1find_package(pcl_conversions REQUIRED)
2find_package(PCL REQUIRED COMPONENTS common io)
3link_directories(${PCL_LIBRARY_DIRS})
4add_definitions(${PCL_DEFINITIONS})

老的tf的一些函数实现

 1#include <geometry_msgs/msg/quaternion.hpp>
 2#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 3#include <tf2/LinearMath/Quaternion.h>
 4
 5namespace tf
 6{
 7
 8inline geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw)
 9{
10    tf2::Quaternion q;
11    q.setRPY(0, 0, yaw);
12    return tf2::toMsg(q);
13}
14
15inline geometry_msgs::msg::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw)
16{
17    tf2::Quaternion q;
18    q.setRPY(roll, pitch, yaw);
19    return tf2::toMsg(q);
20}
21
22inline tf2::Quaternion createQuaternionFromYaw(double yaw)
23{
24    tf2::Quaternion q;
25    q.setRPY(0, 0, yaw);
26    return q;
27}
28
29inline tf2::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
30{
31    tf2::Quaternion q;
32    q.setRPY(roll, pitch, yaw);
33    return q;
34}
35}

tf2没有tf::Pose,改用tf2::Transform

<tf2_geometry_msgs/tf2_geometry_msgs.h>中包含消息和tf2数据转换

tf2::fromMsg()tf2::toMsg()

  • geometry_msgs::msg::Pose<=>tf2::Transform
  • geometry_msgs::msg::Quaternion<=>tf2::Quaternion
  • geometry_msgs::msg::TransformStamped<=>tf2::Stamped<tf2::Transform>

另外还有把消息里面的timestamp转tf2的tf2::TimePointtf2_ros::fromMsg

tf2::getYawtf2::getEulerYPR<tf2/utils.h>里面

spdlog冲突

如果使用自己编译封装的spdlog库打印日志,rclcpp::init需要添加参数才不会冲突

1rclcpp::InitOptions options;
2options.auto_initialize_logging(false);
3rclcpp::init(argc, argv, options);

ROS2CLion

https://www.jetbrains.com/help/clion/ros2-tutorial.html

ROS2适配CLion的顶层CMakeLists.txt

用这种方法不需要参考CLion官方的导入compile_commands.json的办法

把该CMakeLists.txt放到工作目录下,和src平级

 1cmake_minimum_required(VERSION 3.14)
 2project("ROS2 Master")
 3
 4# usually I put this in a separate file include("/opt/ros/_common/Colcon.cmake")
 5function(colcon_add_subdirectories)
 6  cmake_parse_arguments(PARSE_ARGV 0 "ARG" "" "BUILD_BASE;BASE_PATHS" "")
 7
 8  message("search criteria: ${ARGV}")
 9
10  execute_process(COMMAND colcon list
11    --paths-only
12    --base-paths ${ARG_BASE_PATHS}
13    --topological-order
14    ${ARG_UNPARSED_ARGUMENTS}
15    OUTPUT_VARIABLE paths)
16    string(STRIP "${paths}" paths)
17    string(REPLACE "\n" ";" paths "${paths}"
18  )
19
20  MESSAGE("colcon shows paths ${paths}")
21
22  foreach(path IN LISTS paths)
23    message("...examining ${path}")
24    # if(EXISTS "${path}/CMakeLists.txt")
25    execute_process(COMMAND colcon info --paths "${path}" OUTPUT_VARIABLE package_info)
26    if(NOT "${package_info}" MATCHES "type:[ \t]+(cmake|ros.ament_cmake|ros.cmake)")
27      message("skipping non-cmake project")
28    elseif(NOT "${package_info}" MATCHES "name:[ \t]+([^ \r\n\t]*)")
29      message(WARNING "could not identify package at ${path}")
30    else()
31      set(name "${CMAKE_MATCH_1}")
32      message("...adding package ${name} from path ${path}")
33      MESSAGE("package info: ${package_info}")
34
35      get_filename_component(BUILD_PATH "${name}" ABSOLUTE BASE_DIR "${ARG_BUILD_BASE}")
36
37      add_subdirectory("${path}" "${BUILD_PATH}")
38    endif()
39  endforeach()
40endfunction()
41
42colcon_add_subdirectories(
43 BUILD_BASE "${PROJECT_SOURCE_DIR}/build"
44 BASE_PATHS "${PROJECT_SOURCE_DIR}/src/"
45  # 指定单独编译包
46# --packages-select locate_node
47)

先编译一遍工程文件,然后source install/setup.bash,然后用命令行启动CLion打开工程选该CMakeLists.txt即可

使用该脚本会碰到自己写的包相互依赖出问题,因为脚本中使用的是add_subdirectory("${path}" "${BUILD_PATH}"),假设两个自己写的包package_a和package_b,如果pacakge_b依赖package_a,如果CMakeLists中出现以下语句会出问题:

 1find_package(package_a REQUIRED)
 2# 上面语句可以改为
 3find_package(package_a QUIET)
 4
 5ament_target_dependencies(${PROJECT_NAME} package_a)
 6上面语句需要在顶层CMake中定义一个变量 USE_AMENT 开关ament,把因为QUIET导入包出问题的ament语句放进去
 7
 8if (USE_AMENT)
 9  ament_target_dependencies(${PROJECT_NAME} package_a)
10endif ()

不建议用该配置,修改后还是会出各种问题,因为不能用ament_target_dependencies了,和用纯CMake写工程差不多

ROS2 colcon跟ROS1的构建工具catkin_make区别在于前者是每个包单独CMake编译,后者是所有包一起CMake编译,catkin build没试过

原脚本地址: https://gist.github.com/rotu/1eac858b808b82bbf1b475f515e91636

tf到tf2

tf::Pose没了,需要用tf2::transform替代

geometry_msgs/PoseStampedtf2::transform的相互转换

 1// 创建一个PoseStamped消息
 2geometry_msgs::msg::PoseStamped pose_stamped_msg;
 3
 4// 转换为tf2::Transform
 5tf2::Transform tf2_transform;
 6tf2::fromMsg(pose_stamped_msg.pose, tf2_transform);
 7
 8// 创建一个tf2::Transform
 9tf2::Transform tf2_transform_new;
10
11// 转换为PoseStamped消息
12geometry_msgs::msg::PoseStamped pose_stamped_msg_new;
13pose_stamped_msg_new.pose = tf2::toMsg(tf2_transform_new);

包里面链接同一包的消息

1if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
2    rosidl_target_interfaces(trajectory_analyzer_node
3    planning_debug_tools "rosidl_typesupport_cpp")
4else()
5    rosidl_get_typesupport_target(
6            cpp_typesupport_target planning_debug_tools "rosidl_typesupport_cpp")
7    target_link_libraries(trajectory_analyzer_node "${cpp_typesupport_target}")
8endif()

这里的trajectory_analyzer_node是节点名,planning_debug_tools是工程名

源码参考: https://github.com/autowarefoundation/autoware.universe/blob/bdd8d65d084b4505a092d97d0b05225e7303eef8/planning/planning_debug_tools/CMakeLists.txt#L29

CMake IN_LIST报错

错误示例:

CMake Error at /opt/ros/humble/share/ament_cmake_core/cmake/core/ament_execute_extensions.cmake:41 (if):
  if given arguments:

    "ament_cmake_core" "IN_LIST" "_ARG_EXCLUDE"

  Unknown arguments specified
Call Stack (most recent call first):
  /opt/ros/humble/share/ament_cmake_core/cmake/core/ament_package.cmake:66 (ament_execute_extensions)
  CMakeLists.txt:60 (ament_package)

CMakeLists.txtproject(xxx)后面加上如下

1if(POLICY CMP0057)
2    cmake_policy(SET CMP0057 NEW)
3endif()

c++ param

[!NOTE] ROS2的param需要先声明后获取,而且launch文件中如果是double类型的值一定要用5.0而非5这种整型形式

例如

1<launch>
2  <node pkg="test_param" exec="test_param">
3    <param name="a" value="0.0" />
4  </node>
5</launch>
 1int main(int argc, char **argv)
 2{
 3    rclcpp::init(argc, argv);
 4    rclcpp::Node node("test_param");
 5    double a = 1.0;
 6    // node.declare_parameter("a", rclcpp::ParameterType::PARAMETER_DOUBLE);
 7    // 这种更方便
 8    node.declare_parameter("a", a);
 9    node.get_parameter("a", a);
10    RCLCPP_INFO(node.get_logger(), "a: %lf", a);
11}
1node.declare_parameter("value_name", default_value);
2bool result = node.get_parameter("value_name", value);

如果不先调用declare_parameter则后面的get_parameter取不到数据

[!NOTE] 一个参数声明一次就够了,多个地方使用的时候不能重复调用declare_parameter,不然程序会异常退出

ROS bag

ros1的bag包转ros2的bag包

https://gitlab.com/ternaris/rosbags

文档: https://ternaris.gitlab.io/rosbags/

1pip install rosbags
2# 然后把`~/.local/bin`加到PATH
3rosbags-convert foo.bag
4rosbags-convert foo.bag --dst /dir/path/to/save
5# 然后在foo目录下能找到.db3的bag包还有metadata.yaml文件

默认存储为db3的sqlite格式,可以用zstd进行进一步压缩

1# 例如
2zstd 2023-11-15-14-31-41.db3
3# output:
4# 2023-11-15-14-31-41.db3 : 54.75%   (3678203904 => 2013660420 bytes, 2023-11-15-14-31-41.db3.zst)

压缩后剩下55%左右,如果指定高等级可以压缩更多

1# 指定level(1-19)等级为10
2zstd -10 2023-11-15-14-31-41.db3

压缩后剩下45%左右

[!TIP] ros2 bag record参数里面也可设置zstd压缩

命令行别名

1# build package
2alias colcon-bp='colcon build --symlink-install --event-handlers console_direct+ --cmake-args -Wno-dev -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja --packages-select'
3# build all
4alias colcon-ba='colcon build --symlink-install --event-handlers console_direct+ --cmake-args -Wno-dev -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja'

把该两条命令放在.bashrc.zshrc里面即可使这两条用命令编译包

正则表达式

使用Python脚本替换一些常用的类型和文件

  1#!/usr/bin/python3
  2
  3# 该脚本用来初步替换代码中的内容为ROS2的写法
  4
  5import re
  6import sys
  7
  8# 创建一个列表,其中包含所有查找和替换的正则表达式对
  9cpp_regex = [
 10    (r'\b(\w*_msgs)\:\:(?!msg)(\w{1,})\b', r'\1::msg::\2'),
 11    (r'\bconst (\w*_msgs\:\:msg\:\:\w*?)(::)?ConstPtr\s*&\s*(\w*)', r'\1::ConstSharedPtr \3'),
 12    (r'(\w*\-\>header\.stamp)\.toSec\(\)', r'rclcpp::Time(\1).seconds()'),
 13    (r'ros\:\:Time\(\)\.fromSec\((laserCloudTime)\)', r'rclcpp::Time(\1).seconds()'),
 14    (r'ros\:\:Time ', r'rclcpp::Time '),
 15    (r'ROS_(\w+)\(((?:.|\n)*?)\);', r'RCLCPP_\1(node->get_logger(), \2);'),
 16    ('<time.h>', '<ctime>'),
 17    ('<math.h>', '<cmath>'),
 18    ('<stdio.h>', '<cstdio>'),
 19    ('<stdlib.h>', '<cstdlib>'),
 20    ('<ros/ros.h>', '<rclcpp/rclcpp.hpp>'),
 21    ('<std_msgs/Bool.h>', '<std_msgs/msg/bool.hpp>'),
 22    ('<std_msgs/Int32.h>', '<std_msgs/msg/int32.hpp>'),
 23    ('<std_msgs/UInt32.h>', '<std_msgs/msg/uint32.hpp>'),
 24    ('<std_msgs/String.h>', '<std_msgs/msg/string.hpp>'),
 25    ('<std_msgs/Int32MultiArray.h>', '<std_msgs/msg/int32_multi_array.hpp>'),
 26    ('<std_msgs/Float32MultiArray.h>', '<std_msgs/msg/float32_multi_array.hpp>'),
 27    ('<std_msgs/ColorRGBA.h>', '<std_msgs/msg/color_rgba.hpp>'),
 28    ('<nav_msgs/Path.h>', '<nav_msgs/msg/path.hpp>'),
 29    ('<nav_msgs/Odometry.h>', '<nav_msgs/msg/odometry.hpp>'),
 30    ('<geometry_msgs/Pose.h>', '<geometry_msgs/msg/pose.hpp>'),
 31    ('<geometry_msgs/Point.h>', '<geometry_msgs/msg/point.hpp>'),
 32    ('<geometry_msgs/Polygon.h>', '<geometry_msgs/msg/polygon.hpp>'),
 33    ('<geometry_msgs/PoseStamped.h>', '<geometry_msgs/msg/pose_stamped.hpp>'),
 34    ('<geometry_msgs/PointStamped.h>', '<geometry_msgs/msg/point_stamped.hpp>'),
 35    ('<geometry_msgs/PolygonStamped.h>', '<geometry_msgs/msg/polygon_stamped.hpp>'),
 36    ('<sensor_msgs/PointCloud2.h>', '<sensor_msgs/msg/point_cloud2.hpp>'),
 37    ('<sensor_msgs/Joy.h>', '<sensor_msgs/msg/joy.hpp>'),
 38    ('<visualization_msgs/Marker.h>', '<visualization_msgs/msg/marker.hpp>'),
 39    ('<tf/transform_datatypes.h>', '<tf2/transform_datatypes.h>'),
 40    ('<tf/transform_broadcaster.h>', '<tf2_ros/transform_broadcaster.h>'),
 41    ('tf::TransformBroadcaster', 'tf2_ros::TransformBroadcaster'),
 42    ('tf::Matrix3x3', 'tf2::Matrix3x3'),
 43    ('ros::Publisher ', 'rclcpp::Publisher<>::SharedPtr '),
 44    ('ros::Subscriber ', 'rclcpp::Subscription<>::SharedPtr '),
 45    ('tf::Vector3', 'tf2::Vector3'),
 46    ('tf::Quaternion', 'tf2::Quaternion'),
 47    ('ros::NodeHandle nh;', 'rclcpp::Node node();'),
 48    ('ros::Rate', 'rclcpp::Rate'),
 49    ('ros::ok', 'rclcpp::ok'),
 50    ('ros::init', 'rclcpp::init'),
 51    ('ros::spin', 'rclcpp::spin'),
 52    ('ros::Timer ', 'rclcpp::TimerBase::SharedPtr '),
 53    ('tf2::StampedTransform', 'geometry_msgs::msg::TransformStamped'),
 54    ('\.publish', '->publish'),
 55    ('boost::bind', 'std::bind'),
 56]
 57
 58cmake_regex = [
 59    (r'cmake_minimum_required\(VERSION (.*)\)', r'cmake_minimum_required(VERSION 3.5)'),
 60]
 61
 62launch_regex = [
 63    # type换成exec
 64    (r'(\<node.*?)type=(.*/?\>)', r'\1exec=\2'),
 65    # 去掉node里面的required
 66    (r'(\<node.*?) required\=\"\w*?\"(.*/?\>)', r'\1\2'),
 67    # 去掉param里面的type
 68    (r'(\<param.*?) type=\"\w*?\"(.*/?\>)', r'\1\2'),
 69    (r'\$\(find ', '$(find-pkg-share '),
 70    (r'\bns\b', 'namespace'),
 71    (r'\$\(arg', '$(var'),
 72    ('pkg\=\"rviz\"', 'pkg=\"rviz2\"'),
 73    ('name\=\"rviz\"', 'name=\"rviz2\"'),
 74    ('exec\=\"rviz\"', 'exec=\"rviz2\"'),
 75]
 76
 77if __name__ == '__main__':
 78    # 如果argc < 2, 则运行测试代码,否则读取argv[1]文件替换文件内容
 79    open_file = len(sys.argv) > 1
 80
 81    if open_file:
 82        text = open(sys.argv[1]).read()
 83    else:
 84        text = "const sensor_msgs::PointCloud2ConstPtr& laserCloud2"
 85
 86    regex_pairs = []
 87    # 根据文件后缀选择正则表达式
 88    if open_file and (sys.argv[1].endswith('.cpp') or sys.argv[1].endswith('.h') or sys.argv[1].endswith('.hpp')):
 89        print('cpp file')
 90        regex_pairs = cpp_regex
 91    elif open_file and (sys.argv[1].endswith('.cmake') or sys.argv[1].endswith('CMakeLists.txt')):
 92        print('cmake file')
 93        regex_pairs = cmake_regex
 94    elif open_file and sys.argv[1].endswith('.launch'):
 95        print('launch file')
 96        regex_pairs = launch_regex
 97
 98    for find, replace in regex_pairs:
 99        text = re.sub(find, replace, text)
100
101    # 保存文件
102    if open_file:
103        with open(sys.argv[1], 'w') as f:
104            f.write(text)
105    else:
106        print(text)

参考

Migrating from ROS 1 to ROS 2