0. 简介

本文档用于记录现ROS1与ROS2之间的区别,以及如何向ROS2移植。整体架构基于该文章

ROS2的教程参见:https://docs.ros.org/en/ros2_packages/rolling/api/


1 工程构建

1.1 CMakeList的编写

ROS2采用ament cmake系统,最主要的区别是原先的catkin Cmake宏被取代

find_package(catkin REQUIRED COMPONENTS 
...)

catkin_package(
  INCLUDE_DIRS 
  LIBRARIES 
  CATKIN_DEPENDS)

add_library(controller src/controller.cpp)
add_dependencies(obot_controller obot_msgs_generate_messages_cpp)

install(TARGETS 
          obot_controller
          obot_controller_node
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

############################################ 改变为

find_package(ament_cmake REQUIRED)
find_package(... REQUIRED)

ament_export_include_directories(include)# 标记导出的include目录对应的目录(这是通过目标install调用中的INCLUDES DESTINATION来实现)。
ament_export_libraries(my_library)# 标记安装的库的位置(这是由ament_export_targets调用中的HAS_LIBRARY_TARGET参数来完成)
ament_export_dependencies(some_dependency)
ament_target_dependencies(${PROJECT_NAME} ${DEPS})

ament_package()

install(TARGETS ${PROJECT_NAME}_node
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)

若想构建一个兼容ROS1/2的CMakeList.txt,在不同ROS版本有区别的部分通过环境变量触发条件编译

if($ENV{ROS_VERSION} EQUAL 2)
########################
## BULD VERSION: ROS2 ##
########################
else()
########################
## BULD VERSION: ROS1 ##
########################
endif()

1.2 package.xml

<buildtool_depend>catkin</buildtool_depend>
<!--对应的包-->
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<!--##################-->
<buildtool_depend>ament_cmake</buildtool_depend>
<build_type>ament_cmake</build_type>
<!--对应的包-->
<build_export_depend>rclpy</build_export_depend>
<exec_depend>rclpy</exec_depend>

若想构建一个兼容ROS1/2的package.xml,在不同ROS版本有区别的部分通过环境变量设置条件

  <export>
    <build_type condition="$ROS_VERSION == 1">catkin</build_type>
    <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
  </export>

1.3 launch 文件

ROS2较ROS1,在launch方面进行了比较大的改动。原先ROS1是使用xml格式来编写launch文件,而ROS2却是用python来编写launch文件.

# 导入库
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

# 定义函数名称为:generate_launch_description
def generate_launch_description():
    ns = os.environ.get('ROBOT_ID')##read namespace from env
    return LaunchDescription([
        Node(
            # 创建Node对象fusion_localizer_node,标明所在位置fusion_localizer,将可执行文件重命名为fusion_localizer_nodes,同时可以将运行节点node放在同一个自定义的命名空间ns当中。
            package='fusion_localizer',
            namespace=ns,
            executable='fusion_localizer_node',
            name='fusion_localizer_nodes',
            parameters,
        ),
    ])

##少量parameters也可以用以下方法
parameters=[    {'use_sim_time': True},
                {'lane_filename': "tianjin_special_lanes.yaml"},
                {'qc_info_topic': "/b_info"},
                {'service_lane_in_buffer': 3.0}]

在这里插入图片描述
或者通过以下办法来实现多个节点的调用

def generate_launch_description():
    # 创建Actions.Node对象li_node,标明李四所在位置
    li4_node = Node(
        package="village_li",
        executable="li4_node"
        )
    # 创建Actions.Node对象wang2_node,标明王二所在位置
    wang2_node = Node(
        package="village_wang",
        executable="wang2_node"
        )
    # 创建LaunchDescription对象launch_description,用于描述launch文件
    launch_description = LaunchDescription([li4_node,wang2_node])
    # 返回让ROS2根据launch描述执行节点
    return launch_description

对于ROS1而言,launch文件的编写则比较简单


<launch> 
       <node pkg="beginner1" name="tempspeak" type="speak" output="screen">
       <remap from="speaker" to="listener"/>
       </node> 
       <node pkg="beginner1" name="templisten" type="listen" output="screen">
       <remap from="listener" to="listener"/>
       </node> 

       <group ns="demo_1">
           <node name="demo_1" pkg="demo_1" type="demo_pub_1" output="screen"/>
           <node name="demo_1" pkg="demo_1" type="demo_sub_1" output="screen"/>
       </group>
       <group ns="demo_2">
           <node name="demo_2" pkg="demo_2" type="demo_pub_2" output="screen"/>
           <node name="demo_2" pkg="demo_2" type="demo_sub_2" output="screen"/>
       </group>

       <rosparam file="***.yaml" command="load" ns="XXX">
</launch>

1.4 parameter参数

我们可以在1.3中看到在launch文件中同样可以对parameter进行少量的赋值,但是大量的parameter参数则是需要通过yaml文件来进行读取

launch形式

# 导入库
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

# 定义函数名称为:generate_launch_description
def generate_launch_description():
    para_dir = os.path.join(get_package_share_directory('fusion_localizer'), 'config', 'turtlesim.yaml')##yaml file contains parameters
    ns = os.environ.get('ROBOT_ID')##read namespace from env
    return LaunchDescription([
        Node(
            # 创建Node对象fusion_localizer_node,标明所在位置fusion_localizer,将可执行文件重命名为fusion_localizer_nodes,同时可以将运行节点node放在同一个自定义的命名空间ns当中。
            package='fusion_localizer',
            namespace=ns,
            executable='fusion_localizer_node',
            name='fusion_localizer_nodes',
            parameters=[para_dir]
        ),
    ])

命令行形式

ros2 run turtlesim turtlesim_node --ros-args --params-file ./turtlesim.yaml

在ROS2中yaml文件格式则是不一样的:第一层为你的namespace第二层为你的node_name第三层为ros__parameters,该层名称为固定不可修改的,第四层为具体的参数,接收包括int, double, bool, string等类型。同一namespace的node应放置于同一层相邻。

your_node_namespace:
  your_node_name_1:
    ros__parameters:
      ip: "192.168.0.10"
      port: 12002
      output: false

  your_node_name_2:
     ros__parameters:
        subscribe_topic: "/node_namespace_1/topic"

而在ROS1中则是很简单的yaml格式,直接是namespace层(可省略)对应的具体的参数。

local_costmap:
  global_frame: odom
  robot_base_frame: base_link

  update_frequency: 5.0
  publish_frequency: 5.0
  transform_tolerance: 0.5

同时,在ROS1中,Parameter参数机制默认是无法实现动态监控的(需要配合专门的动态机制),比如正在使用的参数被其他节点改变了,如果不重新查询的话,就无法确定改变之后的值。ROS2最新版本中添加了参数的事件触发机制ParameterEventHandler, 当参数被改变后,可以通过回调函数的方式,动态发现参数修改结果。这里古月老师讲的很详细了,具体参照这篇文章


2 代码移植

ROS2中有较多细节上的变化,开始coding之前应当注意。尤其是ROS2代码编写中节点通讯、文件中include调用msg、namespace有变化:

2.1 Node

ROS2中实现了rclcpp::Node类,因此功能节点的实现采用主要的类继承Node的方式,不再需要维护node handle。而Node类提供了clock、time、logger、create publisher/subscriber/timer/server/client 等一系列功能。参考链接:

https://docs.ros2.org/beta3/api/rclcpp/classrclcpp_1_1node_1_1Node.html

2.1.1 publisher/subscriber

在继承了Node的类中声明publisher/subscriber的智能指针成员

rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr my_sub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr my_pub_;

在类构造函数中创建publisher/subscriber,在create_subscription中使用std::bind或者lambda表达式构造调用对象Callback,当然是用ROS1的方式也可以编写简单的ROS1程序,只是ROS2已经趋向推荐面向对象编程了。

my_pub_ = this->create_publisher<std_msgs::msg::String>("/localization_permutation", 10);

//--------------------create_subscription-----------------------------
my_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
                    localization_topic, 1, 
                    std::bind(&LocalizationSwitchboard::localizationCallback, 
                              this, std::placeholders::_1));
//或者
my_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
                    localization_topic, 1, 
                    [this](const nav_msgs::msg::Odometry::ConstPtr msg)
                    {localizationCallback(msg)};

//--------------------/create_subscription-----------------------------
//callback function
void LocalizationSwitchboard::localizationCallback(const nav_msgs::msg::Odometry::ConstPtr msg){
//do stuff
}

示例


#include "rclcpp/rclcpp.hpp"

/*
    创建一个类节点,名字叫做SingleDogNode,继承自Node.
*/
class SingleDogNode : public rclcpp::Node
{

public:
    // 构造函数,有一个参数为节点名称
    SingleDogNode(std::string name) : Node(name)
    {
        // 打印一句自我介绍
        RCLCPP_INFO(this->get_logger(), "大家好,我是单身狗%s.",name.c_str());
    }

private:

};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*产生一个Wang2的节点*/
    auto node = std::make_shared<SingleDogNode>("wang2");
    /* 运行节点,并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

2.1.2 service/client

在继承了Node的类中声明service/client的智能指针成员

rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr switch_srv_;
rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr ndt_client_;

在类构造函数中创建service/client

auto callback_ = std::bind(&LandMarkLocalizer::SwitchService, this, std::placeholders::_1, std::placeholders::_2);

switch_srv_ = this->create_service<std_srvs::srv::SetBool>("ndt_lidar_localizer/switch", 
callback_);

ndt_client_ = this->create_client<std_srvs::srv::SetBool>("ndt_lidar_localizer/switch");

service端部分对request的处理方式与ros1一致,但client端发送请求获得反馈的方式有变化
ROS1中常用的client.call(srv)方式不再被ROS2支持,转而使用client->async_send_request(rquest)的异步方式。这种方式会返回一个std::future类型的变量用于检测是否已经获得回应。

在实际移植过程中发现,一个常见的使用场景:client在原地阻塞式的等待request结果将会造成死锁。原因是因为ROS2中也是默认每一个Node都是一个进程。哪怕不显式的注册,async_send_request也会有一个callback用来接收response。这样就造成了在timer callback中等待会在其他callback中到达的response。而ROS2中若不采用多线程、multi callback group的spin方式,各个callback间采用单线程轮询调度,get()会阻塞当前线程,async_send_request的callback就永远无法被调用。解决方式就是采用多线程、multi callback group的方式spin。这里我们可以使用r.wait_for(1s);来有效避免延迟导致的问题。

auto r = ins_client->async_send_request(new_req_ptr, visual_lane_tracker_on_client_callback);
auto status = r.wait_for(1s);
if (status != std::future_status::ready) RCLCPP_ERROR_STREAM(this->get_logger(),"Time out waiting for response: " << name);

示例服务端

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

#include <memory>

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);

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");

  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();
}

示例客户端

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

#include <chrono>
#include <cstdlib>
#include <memory>

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;
  }

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_client");
  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client =
    node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");

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

  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...");
  }

  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;
}

2.1.3 Action

action在发布和接收层面均没有太大的变化,但是整体结构基本发生了变化,这部分从ROS1转到ROS2建议直接对着官方文档重写。

示例服务端

#include <inttypes.h>
#include <memory>
#include "example_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'
#include "rclcpp_action/rclcpp_action.hpp"

// 创建一个ActionServer类
class MinimalActionServer : public rclcpp::Node
{
public:
  using Fibonacci = example_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

  explicit MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("minimal_action_server", options)
  {
    using namespace std::placeholders;

    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
      this->get_node_base_interface(),
      this->get_node_clock_interface(),
      this->get_node_logging_interface(),
      this->get_node_waitables_interface(),
      "fibonacci",
      std::bind(&MinimalActionServer::handle_goal, this, _1, _2),
      std::bind(&MinimalActionServer::handle_cancel, this, _1),
      std::bind(&MinimalActionServer::handle_accepted, this, _1));
  }

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

  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;
    // Let's reject sequences that are over 9000
    if (goal->order > 9000) {
      return rclcpp_action::GoalResponse::REJECT;
    }
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  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 execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Executing goal");
    rclcpp::Rate loop_rate(1);
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Fibonacci::Feedback>();
    auto & sequence = feedback->sequence;
    sequence.push_back(0);
    sequence.push_back(1);
    auto result = std::make_shared<Fibonacci::Result>();

    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
      // Check if there is a cancel request
      if (goal_handle->is_canceling()) {
        result->sequence = sequence;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal Canceled");
        return;
      }
      // Update sequence
      sequence.push_back(sequence[i] + sequence[i - 1]);
      // Publish feedback
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "Publish Feedback");

      loop_rate.sleep();
    }

    // Check if goal is done
    if (rclcpp::ok()) {
      result->sequence = sequence;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "Goal Succeeded");
    }
  }

  void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    using namespace std::placeholders;
    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
    std::thread{std::bind(&MinimalActionServer::execute, this, _1), goal_handle}.detach();
  }
};  // class MinimalActionServer

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

  auto action_server = std::make_shared<MinimalActionServer>();

  rclcpp::spin(action_server);

  rclcpp::shutdown();
  return 0;
}

示例客户端


#include <inttypes.h>
#include <memory>
#include <string>
#include <iostream>
#include "example_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'
#include "rclcpp_action/rclcpp_action.hpp"

class MinimalActionClient : public rclcpp::Node
{
public:
  using Fibonacci = example_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;

  explicit MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
  : Node("minimal_action_client", node_options), goal_done_(false)
  {
    this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
      this->get_node_base_interface(),
      this->get_node_graph_interface(),
      this->get_node_logging_interface(),
      this->get_node_waitables_interface(),
      "fibonacci");

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

  bool is_goal_done() const
  {
    return this->goal_done_;
  }

  void send_goal()
  {
    using namespace std::placeholders;

    this->timer_->cancel();

    this->goal_done_ = false;

    if (!this->client_ptr_) {
      RCLCPP_ERROR(this->get_logger(), "Action client not initialized");
    }

    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
      this->goal_done_ = true;
      return;
    }

    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(&MinimalActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
      std::bind(&MinimalActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
      std::bind(&MinimalActionClient::result_callback, this, _1);
    auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

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

  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)
  {
    RCLCPP_INFO(
      this->get_logger(),
      "Next number in sequence received: %" PRId32,
      feedback->sequence.back());
  }

  void result_callback(const GoalHandleFibonacci::WrappedResult & result)
  {
    this->goal_done_ = true;
    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;
    }

    RCLCPP_INFO(this->get_logger(), "Result received");
    for (auto number : result.result->sequence) {
      RCLCPP_INFO(this->get_logger(), "%" PRId32, number);
    }
  }
};  // class MinimalActionClient

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto action_client = std::make_shared<MinimalActionClient>();

  while (!action_client->is_goal_done()) {
    rclcpp::spin_some(action_client);
  }

  rclcpp::shutdown();
  return 0;
}

2.2 Timer

在类中声明TimerBase的智能指针成员

rclcpp::TimerBase::SharedPtr timer_;

有两种方式实例化timer,区别是时钟源

//wall timer使用系统时间
timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&LandMarkLocalizer::LandmarkThread, this));
//抽象化的timer,可以自定义时钟源。此处给的是node clock 即ros clock,在调试时可以使用仿真时间
landmark_timer_ = rclcpp::create_timer(this, this->get_clock(), std::chrono::milliseconds(100),std::bind(&LandMarkLocalizer::LandmarkThread, this));

2.3 Parameters

注意在代码中访问nested params应采用”.”作为层级分割符,而不是“/”

//定义名字为odom/topic_name的参数默认值为odom0里面的值
ros_node_->declare_parameter("odom.topic_name", rclcpp::ParameterValue("odom0"));
ros_node_->get_parameter("odom.topic_name", temp,1);//无法获取时用默认值

可参考: https://roboticsbackend.com/ros2-yaml-params/

示例

#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <string>
#include <functional>

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;
}

3 ROS2在调用层面的变化

3.1 msg/srv namespace和头文件的变化

#include <nav_msgs/Odometry.h>
#include <std_srvs/SetBool.h>
#include <std_msgs/Int32.h>

<std_srvs::SetBool>
<std_msgs::String>

//-------------变为----------------------
#include <nav_msgs/msg/odometry.hpp>
#include <std_srvs/srv/set_bool.hpp>
#include <std_msgs/msg/int32.hpp>

<std_srvs::srv::SetBool>
<std_msgs::msg::String>

3.2 LOG的变化

在打印log之前先获取logger

ROS_INFO("...");
-----------------
RCLCPP_INFO(this->get_logger(),"...")

3.3 时间(戳)的变化

ROS2中msg不再支持ROS1中的基本类型time,因此所有相关时间戳使用builtin_interfaces/Time类型替代

//从时间戳获取浮点型的时间,先利用stamp构造rclcpp::Time对象
double stamp = rclcpp::Time(sensor_data->dji_points->header.stamp).seconds();
//获取当前时间
double start_stamp = this->now().seconds();

4. 参考链接

https://www.guyuehome.com/35373
https://github.com/yszheda/wiki/wiki/ros2
http://blog.chinaunix.net/uid-27875-id-5834655.html
https://blog.csdn.net/yang434584531/article/details/115749810
http://blog.chinaunix.net/uid-27875-id-5834655.html
https://blog.csdn.net/flyfish1986/article/details/82986168