[ROS] 13. 퍼블리셔-서브스크라이버 기능을 한 개의 노드로 구현

☞ 메인보드 : Jetson Nano Developer Kit

☞ 운영 체제 : Ubuntu 18.04 - JetPack 4.4.1

☞ ROS 버전 : Melodic

☞ IDE : Visual Studio Code

☞ 언어 : C++

 

 

 

 

---

목차

○ 1. 패키지 생성

○ 2. 코드 작성

○ 3. 실행 결과

○ 4. 사용자 설정 메세지 통신

 

 

 

---

 

 

 

이전에는 ROS에 대해 이해도가 부족하여 메세지 통신 노드를 생성할 때 퍼블리셔(발행) 노드 한 개, 서브스크라이버(구독) 노드 한 개로 2 개의 노드를 생성하여 기본적으로 2 개 이상의 노드를 사용했다. 하지만, 필요한 경우 한 노드에도 여러 기능을 넣어 사용할 수 있다는 것을 알고 난 후에는 퍼블리셔-서브스크라이버, 서브스크라이버-서버 등 다양한 조합으로 한 노드에 기능을 넣어 사용하게 되었다.

 

ROS에서 제공하는 std_msgs 패키지를 사용하지 않고 2 가지 이상의 메세지 타입을 사용해 통신을 하고 싶을 때는 사용자 설정(Custom) 통신을 하면 된다. 이번 포스트에는 퍼블리셔-서브스크라이버 기능을 하나의 노드에 작성하고 사용자 설정 통신에 대한 내용에 대해 다뤄볼 것이다.

 

[ROS] 11. Lidar Sensor

 

이전에 발행했던 위 포스트에는 라이다 센서에 대한 내용을 적어놓았는데 이번 포스트에서 라이다 센서에서 반환하는 angle, distance 값을 구독하고 다시 발행하는 예제를 만드려한다. 사실 YDLIDAR 사에서 제공하는 ydlidar_client 파일에서 숫자만 바꾸면 노드를 새로 만들 필요가 없지만 하나의 노드가 두 가지의 기능을 하는 것을 보기 위해서 만드는 것이라 효율적이지 않다고 한다면 다음에는 더 쓸모있는 예제를 만들어 오도록 노력하겠다...ㅎ.ㅎ

 

 

 

 

 

① 패키지 생성

# 패키지 생성
cd ~/catkin_ws/src
catkin_create_pkg multi_node roscpp std_msgs sensor_msgs message_generation

# catkin_make를 위해 ~/catkin_ws 디렉터리로 이동
cd ..
catkin_make

 

패키지를 생성할 때는 기존 퍼블리셔나 서브스크라이버 노드를 작성할 때와 같이 작성하면 된다. 사용자 설정 통신으로 메세지 통신을 하고 싶으면 별도로 패키지 내의 msg 디렉터리를 생성하고, 서비스 통신을 하고 싶다면 srv 디렉터리를 생성하고 그 안에 .msg과 .srv 확장자의 파일을 작성해주면 된다. 코드를 작성할 때 헤더 파일을 추가해야 사용할 수 있는데 catkin_make 과정을 거치면 자동으로 생긴다. 이는 직접 만들어보면 금방 알 수 있다.

 

 

 

---

② 코드 작성[pubSub.cpp]

 

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include <multi_node/LidarInfoMsg.h>
#include <iostream>

#define RAD2DEG(x) ((x)*180./M_PI)

class PubAndSub
{
private:
    ros::NodeHandle n_;
    ros::Publisher pub_;
    ros::Subscriber sub_;

public:
    PubAndSub()
    {
        pub_ = n_.advertise<multi_node::LidarInfoMsg>("/sensor_value", 1000);
        sub_ = n_.subscribe("/scan", 1000, &PubAndSub::scanCallback, this);
    }

    void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
    {
        multi_node::LidarInfoMsg data;
        int count = scan->scan_time / scan->time_increment;
        for(int i = 0; i < count; i++) {
            float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
            data.angle = (int)degree;
            data.distance = scan->ranges[i];
            if(data.angle >= -70 && data.angle <= 70) {	if(data.angle % 10 == 0) { pub_.publish(data); } }
        }
    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "multi_node");
    PubAndSub PAS;
    ros::spin();
    return 0;
}

 

 

코드 작성은 노드를 한 개만 만들어 사용했을 때와 비슷하다. 퍼블리셔, 서브스크라이버 역할을 위해 advertise와 subscribe를 각각 선언해준다. scnaCallback 함수에는 발행, 구독에 대한 내용이 함께 들어가면 된다. 이 코드의 경우 ydlidar_client 의 /scan 토픽 구독으로 받아온 angle , distance 값을 사용자 설정 메시지 파일에 저장해둔 data.angle, data.distance 에 저장하고 이를 발행하는 코드이다.

 

사실 scanCallback 함수의 내용은 ydlidar_client에 있는 내용과 숫자만 다르다. YDLIDAR 깃허브에서 해당 패키지를 다운로드 하면 조건문 안에 degree 값이 -5에서 +5 사이의 값만 받게 되어있다. 이 범위를 -90에서 +90으로 늘려주면 180이나 되는 각도의 거리를 라이다 센서로 부터 받을 수 있다.

 

 

// ydlidar_client.cpp
if(degree > 90 && degree < 90) {
	printf("[YDLIDAR INFO]: angle-distance : [%i, %f, %i]\n", degree, scan->ranges[i], i);
}


// pubSub.cpp
if(data.angle >= -70 && data.angle <= 70) {	
	if(data.angle % 10 == 0) {
    	pub_.publish(data); 
    }
}

 

pusSub.cpp 에서는 ydlidar_client로 부터 받은 -90도에서 90도 angle 값을 -70에서 70 사이의 값만 받되 이 중에 10도 단위로 값(-10, 0 , 10 과 같이)을 다시 발행한다.

 

 

 

 

 

아래에 더보기 버튼을 클릭하면 ydlidar_client.cpp 내용을 볼 수 있다.

※ ydlidar_client.cpp

 

/*
 *  YDLIDAR SYSTEM
 *  YDLIDAR ROS Node Client 
 *
 *  Copyright 2015 - 2018 EAI TEAM
 *  http://www.ydlidar.com
 * 
 */

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"

#define RAD2DEG(x) ((x)*180./M_PI)

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    int count = scan->scan_time / scan->time_increment;
    printf("[YDLIDAR INFO]: I heard a laser scan %s[%d]:\n", scan->header.frame_id.c_str(), count);
    printf("[YDLIDAR INFO]: angle_range : [%f, %f]\n", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));
  
    for(int i = 0; i < count; i++) {
        float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
	if(degree > 90 && degree < 90)
        printf("[YDLIDAR INFO]: angle-distance : [%i, %f, %i]\n", degree, scan->ranges[i], i);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ydlidar_client");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);
    ros::spin();

    return 0;
}

 

 

 

---

③ XML, CMAKE 파일 작성 +  메세지 통신 파일 작성

 

※ XML

 

<?xml version="1.0"?>
<package format="2">
  <name>multi_node</name>
  <version>0.0.0</version>
  <description>The multi_node package</description>
  <maintainer email="yasun95@todo.todo">yasun95</maintainer>
  <license>TODO</license>
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <export>
  </export>
</package>

 

 

 

 

※ CMAKE

 

cmake_minimum_required(VERSION 3.0.2)
project(multi_node)
find_package(catkin REQUIRED COMPONENTS
  message_generation
  roscpp
  sensor_msgs
  std_msgs
)

add_message_files(
  FILES 
  LidarInfoMsg.msg
)

generate_messages(
  DEPENDENCIES 
  std_msgs 
  sensor_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES multi_node
#  CATKIN_DEPENDS message_generation roscpp std_msgs
#  DEPENDS system_lib
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

add_executable(${PROJECT_NAME} src/pubSub.cpp)
add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})

 

 

 

 

 

※ 메세지 파일 (LidarInfoMsg.msg)

 

 

 

별거 없다. 이전 서비스 통신의 요청과 응답 방식에는 --- 을 기준으로 요청에 사용되는 변수와 응답에 사용되는 변수로 나누었는데 메세지 통신에서는 그리할 필요가 없다. 구분없이 필요한 변수만 정의해주면 된다.

 

 

int16 angle
float32 distance

 

 

 

 

 

 

 

 

 

---

④ 실행 결과

// 터미널 창 3개 실행

// 첫 째
roscore

// 둘 째 
rosrun ydlidar_ros ydlidar_client

// 셋 째
rosrun multi_node multi_node

 

 

※  ydlidar_client의 /scan 토픽

 

rostopic echo /scan

 

 

※ multi_node의 /sensor_value 토픽

 

rostopic echo /sensor_value

 

 

 

 

 

※ rqt_graph

 

 

 

/sensor_value 토픽은 Dead_sinks 체크를 해제하면 보인다. 아무도 구독하고 있지 않아 처음에 rqt_graph를 열었을 때는 표시되지 않는다. 이로써 /multi_node가 퍼블리셔, 서브스크라이버를 동시에 할 수 있다는 것을 알 수 있었다.