12. [Option] C++ 코드, template

이번 강의, 그리고 다음 강의에 걸쳐서 앞서 구현했던 다양한 ROS 프로그램들을 C++로 다시 작성해보는 시간을 갖겠습니다.

강좌를 따라옴에 있어 문법적인 측면에선, C++ Class를 사용하는 방법 정도만 알고 계시면 따라오실 수 있습니다.

이와 더불어 ROS는 기본적으로 빌드 툴로 CMake를 사용하기에 CMakeLists.txt에 대한 이해가 있다면 더 좋습니다.

그럼, 시작해보겠습니다!!

basic_node.cpp

#include <ros/ros.h>

int main(int argc, char** argv){

    ros::init(argc, argv, "basic_node");
    ros::NodeHandle nh;
    ros::Rate r(5); # Hz

    while ( ros::ok() ) {
        ROS_INFO("This is Basic Node");
        r.sleep();
    }

    return 0;
}

import rospy
rospy.init_node("drive_forward")
r = rospy.Rate(1)  # 1 Hz
r.sleep()
while not rospy.is_shutdown():
rospy.loginfo()

ros::Rate, r.sleep() 등 익숙한 코드들이 보이지요?

node의 생성은 ros::init으로 가능하며, roscpp에서는 NodeHandle이라는 클래스를 생성해야 Publisher, Subscriber, serviceServer 등의 node를 만들 수 있습니다.

💡

NodeHandle은 반드시 ros::init 보다 뒤에 생성되어야 합니다

ROS 패키지 빌드하기

C++로 작성된 코드는 빌드가 필요하며 caktin 시스템에서는 CMake가 빌드를 도와줍니다.

빌드 시에 ROS와 관련된 공유 라이브러리, 외부 라이브러리들이 필요할 것임을 담고 있는 부분이 CMakeLists.txt 이며 자주 수정이 필요할 것입니다.

CMake에 대해서는 자세히 다루지 않고, 기존 CMakeLists.txt 와 차이가 있는 부분만 살펴보겠습니다.

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  geometry_msgs
  sensor_msgs
)

바로 find_package 키워드로 ROS에서 기본 제공하는 패키지들을 손쉽게 추가할 수 있다는 점입니다.

💡

관련된 패키지는 주로 초기 패키지 생성 시에 다음과 같이 손쉽게 import 가능합니다. catkin_create_pkg my_package <package1> roscpp

빌드 후 생성된 실행 파일 이름을 가지고 launch file이나 rosrun 시 실행시킬 수 있겠지요?

이 실행 파일 이름은 다음 부분에서 확인 가능합니다.

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(basic_node src/basic_node.cpp)
add_executable(cmd_vel_pub_node src/cmdvel_pub.cpp)
add_executable(laser_sub_node src/laser_sub.cpp)
add_executable(parking_node src/parking.cpp)

...
add_dependencies(basic_node ${basic_node_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(cmd_vel_pub_node ${cmd_vel_pub_node_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(laser_sub_node ${laser_sub_node_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(parking_node ${parking_node_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

...
target_link_libraries(basic_node
  ${catkin_LIBRARIES}
)
target_link_libraries(cmd_vel_pub_node
  ${catkin_LIBRARIES}
)
target_link_libraries(laser_sub_node
  ${catkin_LIBRARIES}
)
target_link_libraries(parking_node
  ${catkin_LIBRARIES}
)

가장 중요한 것!!! `source devel/setup.bash`

파이썬과는 다르게, roscpp은 코드 수정시마다 빌드를 새로 해줘야 합니다.

이와 더불어, 빌드 후에도 변경된 내용을 업데이트 해줘야 하며 이는 다음 커맨드로 가능합니다.

(build done...)

$ cd ~/gcamp_ws
$ source devel/setup.bash
# 또는
$ sds

Tip!! 하나의 패키지만 빌드하기

기본적으로 catkin_make를 하게되면 workspace안에 있는 모든 패키지들을 빌드합니다. 저 또한 이 방법이 탐탁치 않아 작업중인 하나의 패키지만 빌드하고자 단축키를 설정하였는데요...

💡

자세한 내용은 강의를 확인하세요!! 😉😉

그럼, topic publisher, subscriber 코드를 살펴보겠습니다.

cmdvel_pub.cpp

/*
 * basic topic publisher example
 * 
 * referenced from wiki.ros.org : http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
 */

#include <chrono>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char** argv){

    ros::init(argc, argv, "cmd_vel_pub_node");
    ros::NodeHandle nh;
    ROS_INFO("==== DriveForward node Started, move forward during 10 seconds ====\n");

    ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 5);
    ros::Rate r(5);

    geometry_msgs::Twist go_forward;
    go_forward.linear.x = 0.5;

    geometry_msgs::Twist stop;
    stop.linear.x = 0.0;

    auto start = std::chrono::steady_clock::now();
    auto now = std::chrono::steady_clock::now();

    std::chrono::duration<double> time_duration = now - start;


    while (time_duration.count() < 5.0){
        cmd_pub.publish(go_forward);
        
        now = std::chrono::steady_clock::now();
        time_duration = now - start;
    }
    
    ROS_WARN(" 5 seconds passed, Stop!! \n");
    cmd_pub.publish(stop);

    return 0;
}

사실상 주의깊게 볼 부분은 다음 한줄입니다.

ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 5);

# python
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)

Publisher 생성 시, 적합한 message를 명기하여 instantiation을 해줘야 합니다.

더불어, 두번째 매개변수는 파이썬 때와 마찬가지로 queue size입니다.

이와 더불어, message를 표현하는 방식이 다르지요?

#include <geometry_msgs/Twist.h>

...
    ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 5);
...

laser_sub.cpp

// basic topic subscriber example
// referenced from wiki.ros.org : http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

void sub_callback(const sensor_msgs::LaserScan &data ){
    std::cout << "Front Lidar Value : " << data.ranges[360] << std::endl;
}

int main(int argc, char** argv) {

    ros::init(argc, argv, "laser_sub_node");
    ros::NodeHandle nh;    
    ros::Subscriber sub = nh.subscribe("/scan", 1, sub_callback);

    ros::spin();

    return 0;
}

template<class M>
ros::Subscriber subscribe(const std::string& topic, uint32_t queue_size,
		 <callback, which may involve multiple arguments>,
		 const ros::TransportHints& transport_hints = ros::TransportHints());

publisher와 달리, instantiation을 생략할 수 있다는 점에 유의합니다.

ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 5);
ros::Subscriber sub = nh.subscribe("/scan", 1, sub_callback);

저의 경우 레퍼런스로 절달했지만, ROS에서는 boost::shared_ptr 형식의 타입들을 기본 제공하고 있답니다. (이 내용은 이해가 안되면 넘어가셔도 좋습니다. 저도 잘 모른답니다 ㅎㅎ)

void sub_callback(const sensor_msgs::LaserScan &data ){
    std::cout << "Front Lidar Value : " << data.ranges[360] << std::endl;
}

# 이것도 가능!
void sub_callback(const sensor_msgs::LaserScanConstPtr &data ){
    std::cout << "Front Lidar Value : " << data.ranges[360] << std::endl;
}

💡

iostream이 없는데 cout이 있네요?

#include <ros/ros.h>

이 부분에서 다음과 같은 헤더 파일들이

include 된답니다 🙃🙃

iostream

string

boost

sstream

vector

assert.h
...

앞선 두 예제들을 합한 것이 parking 예제였지요? 바로 살펴봅시다!

parking.cpp

// Park your robot with lidar distance value
// 
// created by kimsooyoung : https://github.com/kimsooyoung

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>

class TinyBot
{
private:
    ros::NodeHandle m_nh;
    ros::Publisher m_pub;
    ros::Subscriber m_sub;

    std::string m_name;
    geometry_msgs::Twist m_cmd_vel;

public:
    TinyBot(const std::string &name_in = "my_tiny"): m_name(name_in) {
        ROS_INFO("Publisher and Subscriber initialized");
        m_pub = m_nh.advertise<geometry_msgs::Twist>("/cmd_vel", 5);
        m_sub = m_nh.subscribe("/scan", 1, &TinyBot::subCallback, this);
    }

    void robotStop(){
        m_cmd_vel.linear.x = 0.0;
        m_pub.publish(m_cmd_vel);
        ROS_INFO(" Parking Done!!"); 
        ros::shutdown();
    }

    void robotGo(){
        m_cmd_vel.linear.x = 0.8;
        m_pub.publish(m_cmd_vel);
    }

    void subCallback(const sensor_msgs::LaserScan &data){

        auto frontLidarPoint = data.ranges[360];

        if ( frontLidarPoint >= 1.0 ){
            this->robotGo();
        }
        else {
            this->robotStop();
        }

        std::cout << "===== front lidar point val : " << frontLidarPoint << " ====" << std::endl;
    }
};

int main(int argv, char** argc) {

    ros::init(argv, argc, "parking_node");
    TinyBot tinybot("gcamp_robo");
    ros::spin();

    return 0;
}

바로 클래스가 등장해서 조금 당황하셨을 수도 있을텐데요, 천천히 살펴보겠습니다.

기능 부분은 사실상 파이썬 예제에서의 로직과 차이가 없습니다.

    void robotStop(){
        m_cmd_vel.linear.x = 0.0;
        m_pub.publish(m_cmd_vel);
        ROS_INFO(" Parking Done!!"); 
        ros::shutdown();
    }

    void robotGo(){
        m_cmd_vel.linear.x = 0.8;
        m_pub.publish(m_cmd_vel);
    }

    void subCallback(const sensor_msgs::LaserScan &data){

        auto frontLidarPoint = data.ranges[360];

        if ( frontLidarPoint >= 1.0 ){
            this->robotGo();
        }
        else {
            this->robotStop();
        }

        std::cout << "===== front lidar point val : " << frontLidarPoint << " ====" << std::endl;
    }

ros::shutdown(); 을 통해 노드 종료가 가능하답니다.

생성자 부분이 더 생소하실 것 같은데요, 레퍼런스의 선언 부분을 첨부합니다.

public:
    TinyBot(const std::string &name_in = "my_tiny"): m_name(name_in) {
        ROS_INFO("Publisher and Subscriber initialized");
        m_pub = m_nh.advertise<geometry_msgs::Twist>("/cmd_vel", 5);
        m_sub = m_nh.subscribe("/scan", 1, &TinyBot::subCallback, this);
    }

클래스 method로 callback을 지원하기 위해 다음과 같이 function pointer와 object를 받는 생성자가 있습니다.

이 또한 모든 것을 이해하려 하시기보다, 클래스 method를 callback으로 사용하고프면 이렇게 쓰나보다, 하고 이해하시면 됩니다.

roscpp: ros::NodeHandle Class Reference

roscpp's interface for creating subscribers, publishers, etc. This class is used for writing nodes. It provides a RAII interface to this process' node, in that when the first NodeHandle is created, it instantiates everything necessary for this node, and when the last NodeHandle goes out of scope it shuts down the node.

http://docs.ros.org/en/kinetic/api/roscpp/html/classros_1_1NodeHandle.html#a317fe4c05919e0bf3fb5162ccb2f7c28

여기서, 클래스로 하지 않고 전역변수로 설정하면 되지 않나? 라는 생각이 드실 수 있는데요. 앞서 배운 내용들을 종합하여 생각해보면

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>

void sub_callback(const sensor_msgs::LaserScan &data ){

    // do Something
    geometry_msgs::Twist pub_msg;

    cmd_pub.publish(pub_msg); // ....?
}

int main(int argv, char** argc) {

    ros::init(argv, argc, "parking_node");
    ros::NodeHandle nh;

    ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 5);
    ros::Subscriber scan_sub = nh.subscribe("/scan", 1, subCallback);

    ros::spin();

    return 0;
}

subscriber는 NodeHandle이 있어야 생성 가능합니다.

NodeHandle은 init이 있어야 생성 가능하지요.

⇒ 그럼 결국 모든 초기화 부분이 main 밖에 존재하게 되는 상황이 발생합니다!! 👹👹

이러한 이유로 ROS 공식 문서에서도 전역변수를 지양하고 있답니다 😉

Globals

Globals, both variables and functions, are discouraged. They pollute the namespace and make code less reusable.

Global variables, in particular, are strongly discouraged. They prevent multiple instantiations of a piece of code and make multi-threaded programming a nightmare.

Most variables and functions should be declared inside classes. The remainder should be declared inside namespaces.

Exception: a file may contain a main() function and a handful of small helper functions that are global. But keep in mind that one day those helper function may become useful to someone else.

다음 시간에 이어서, Service, Action 코드를 살펴보겠습니다!!

저작자표시 비영리 동일조건

Swiming_Lab

12. [Option] C++ 코드, template

12. [Option] C++ 코드, template

basic_node.cpp

ROS 패키지 빌드하기

가장 중요한 것!!! `source devel/setup.bash`

Tip!! 하나의 패키지만 빌드하기

cmdvel_pub.cpp

laser_sub.cpp

parking.cpp

Globals

티스토리툴바

12. [Option] C++ 코드, template

basic_node.cpp

ROS 패키지 빌드하기

가장 중요한 것!!! source devel/setup.bash

Tip!! 하나의 패키지만 빌드하기

cmdvel_pub.cpp

laser_sub.cpp

parking.cpp

Globals

티스토리툴바

가장 중요한 것!!! `source devel/setup.bash`