延續昨天的主題，今天要來探討C++版本的Node，不過C++的使用上就比較搞剛一些。我會把說明附在comment裡面，這樣可以直接對照每一行的功能。
另外，從今天開始會帶到一些Command Line Tools(CLI)，這邊會用到ros2 pkg和ros2 node，這兩個指令可以幫助我們檢查Package和執行Node。

C++ Node

C++的API從ROS的roscpp改成ROS2的rclcpp，所以在include的時候要注意。這邊先來簡單的創一個Package和Hello World的Node。

創建Package

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake --node-name hello_world beginner_tutorials_cpp

跟昨天Python一樣，我們在創建package時，可以用--node-name來創建一個初始的node，並在CMakeLists.txt內幫你寫好executable，這裡是hello_world_node。

創建完後，會在~/ros2_ws/src底下看到beginner_tutorials_cpp這個資料夾，裡面有：

beginner_tutorials_cpp/
├── CMakeLists.txt
├── include
│   └── beginner_tutorials_cpp
├── package.xml
├── src
    └──  hello_world.cpp

其中hello_world.cpp就是我們的ROS Node，今天不會寫到Class，所以include底下的beginner_tutorials_cpp目前是空的，否則一般會放一個hello_world.hpp。

撰寫 Hello World Node

1. 首先編輯hello_world.cpp：

   #include "rclcpp/rclcpp.hpp"
   
   // Node header file，目前沒有特別拆開所以不用include
   // #include "beginner_tutorials_cpp/hello_world.hpp"
   
   int main(int argc, char * argv[])
   {
       rclcpp::init(argc, argv);   // 初始化ROS
   
       // 創建一個叫做hello_world_node的Node 
       auto node = rclcpp::Node::make_shared("hello_world_node");  
   
       // 用Node的get_logger() function來print出Hello World!
       RCLCPP_INFO(node->get_logger(), "Hello World!");
   
       // 讓Node持續運行
       rclcpp::spin(node);
   
       // 關閉ROS
       rclcpp::shutdown();
   
       return 0;
   }
   

2. 再來編輯CMakeLists.txt：

   分別在對應的位置加入find_package(rclcpp REQUIRED)和ament_target_dependencies(hello_world_node rclcpp)。

   不熟悉CMakeLists.txt的話可以參考Day4 ROS2 Package - C++。

   cmake_minimum_required(VERSION 3.5)
   project(beginner_tutorials_cpp)
   
   # Default to C++14
   if(NOT CMAKE_CXX_STANDARD)
       set(CMAKE_CXX_STANDARD 14)
   endif()
   
   ...
   
   # find dependencies
   find_package(ament_cmake REQUIRED)
   find_package(rclcpp REQUIRED)
   
   ...
   
   # 增加一個executable，名稱叫做hello_world_node，並且link rclcpp
   add_executable(hello_world_node src/hello_world.cpp)
   target_link_libraries(hello_world_node PUBLIC
       $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
       $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(hello_world_node rclcpp)
   
   # install這個executable
   install(TARGETS
       hello_world_node
       DESTINATION lib/${PROJECT_NAME}
   )
   
   ...
   
   # install這個package
   install(DIRECTORY
       include/
       DESTINATION include/
   )
   
   ament_package()
   

3. 最後編輯package.xml：

   將<build_depend>rclcpp</build_depend>和<exec_depend>rclcpp</exec_depend>放入對應的位置：

   ...
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>rclcpp</build_depend>
   
   <exec_depend>rclcpp</exec_depend>
   ...
   

執行

回到Workspace，執行build：

cd ~/ros2_ws
colcon build --packages-select beginner_tutorials_cpp

⚠️ --symlink-install這邊對C++沒有用，所以每次修改完程式碼後都要重新colcon build。

執行完後記得先source：

source ~/ros2_ws/install/setup.bash

執行：

ros2 run beginner_tutorials_cpp hello_world_node

就可以跟昨天一樣看到Hello World!了。

持續執行

和昨天一樣可以用Loop來讓Node持續輸出Hello World，但是C++的寫法跟Python不太一樣，這邊來簡單的介紹一下。

ROS 寫法

首先第一個方法，跟ROS寫法比較接近，使用rate + loop:

#include "rclcpp/rclcpp.hpp"

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);   // 初始化ROS
    
    // 創建一個叫做hello_world_node的Node 
    auto node = rclcpp::Node::make_shared("hello_world_node");  
    
    // 用Node的get_logger() function來print出Hello World!
    RCLCPP_INFO(node->get_logger(), "Hello World!");
    
    // use rate to loop at 1Hz
    rclcpp::WallRate loop_rate(1);

    // 讓Node持續運行
    while(rclcpp::ok()) {
        rclcpp::spin_some(node);
        RCLCPP_INFO(node->get_logger(), "Hello World in Loop!");
        loop_rate.sleep();
    }
    
    // 關閉ROS
    rclcpp::shutdown();
    
    return 0;
}

首先了解ROS的人會先發現，我們已經慢慢進入Smart Pointer的世界了。在ROS2中，Node的型別是指針，所以不是用傳統的Object oriented的寫法，而是用make_shared()來創建一個Node。這邊的make_shared是C++的Smart Pointer，可以參考Geeks for geeks 的這篇。

再來可以到看，ROS的ros::spinOnce已經被rclcpp::spin_some取代了，而ROS C++中的ros::Rate則是被rclcpp::WallRate取代了。

ROS2 寫法

第二個方法比較進階，但也是ROS2 callback function的寫法，也可以使用Lambda取代callback function。以下列出兩種寫法，把其中一種註解掉就可以執行另一種：

#include "rclcpp/rclcpp.hpp"
#include <chrono>

void callback(rclcpp::Node::SharedPtr node) {
    RCLCPP_INFO(node->get_logger(), "Hello World in Loop!");
}

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);   // 初始化ROS
    
    // 創建一個叫做hello_world_node的Node 
    auto node = rclcpp::Node::make_shared("hello_world_node");  
    
    // 用Node的get_logger() function來print出Hello World!
    RCLCPP_INFO(node->get_logger(), "Hello World!");

    callback function寫法
    auto timer = node->create_wall_timer(
        std::chrono::seconds(1),
        std::bind(&callback, node)
    );

    // // 創建一個Timer，每秒執行一次，Lambda寫法
    // auto timer = node->create_wall_timer(
    //     std::chrono::seconds(1), 
    //     [&node]() -> void {
    //         RCLCPP_INFO(node->get_logger(), "Hello World!");
    //     }
    // );
    
    // 讓Node持續運行
    rclcpp::spin(node);
    
    // 關閉ROS
    rclcpp::shutdown();
    
    return 0;
}

這邊T我們用node->create_wall_timer()來創建一個Timer，第一個參數是時間，第二個參數是一個function，可以是callback function或是Lambda。這邊我們用std::bind()來把callback function綁定到node。Lambda的話則是直接用[&node](){}來寫。

完成後記得要重新colcon build，然後執行：

ros2 run beginner_tutorials_cpp hello_world_node

就可以看到每秒print一次Hello World!了。

ROS2指令 Command Line Tools(CLI)

ROS2的指令跟ROS的指令有些不同，這邊來簡單的介紹一下。

ros2 pkg

ros2 pkg可以幫助我們快速的創建Package，也可以查看Package的資訊。以我們的實作為例，我們可以用ros2 pkg來查看他的資訊：

ros2 pkg list | grep beginner_tutorials

可以看到我們的beginner_tutorials_cpp和beginner_tutorials_py。

如果把| grep beginner_tutorials拿掉，可以看到所有的ROS2 Package。

另外還可以查看可執行的ROS Node:

ros2 pkg executables beginner_tutorials_cpp

ros2 node

ros2 node可以查看目前正在執行的Node，也可以查看Node的資訊。以我們的實作為例，我們可以用ros2 node來查看目前正在執行的Node。但首先要先執行一個Node，這邊我們用ros2 run來執行hello_world_node：

ros2 run beginner_tutorials_cpp hello_world_node

接著，我們可以用ros2 node list來查看目前正在執行的Node：

ros2 node list

可以看到目前正在執行的Node有/hello_world_node。再來我們可以用ros2 node info來查看Node的資訊：

ros2 node info /hello_world_node

這個指令也可以用來檢查Node是否訂閱或發佈某個Topic，或是是否提供或使用某個Service，後面會再介紹。

最後，我們可以用ros2 node -h來查看ros2 node的其他指令：

ros2 node -h

ROS vs. ROS2

Reference

• 蛤 - Day 07 - 使用C++撰寫ROS Node
• ROS2 Official Tutorial - Creating a package
• Writing a simple publisher and subscriber (C++)
• C++ ROS Client Library API