ㅇ배경
ros2에 편리한 내장 메세지나 서비스가 많지만 개발을 하다보면 커스텀 메세지나 서비스를 작성해야 한다. rosidl generator 덕분에 빌드 한 번하면 build 파일 내에 파이썬 ,C++ 버전의 메세지 클래스, 헤더파일이 생성된다.
예제
cd ~/dev_ws
ros2 pkg create --build-type ament_cmake my_msgs
my_msgs라는 패키지를 만들고 하위 폴더로 msg와 srv를 만든다.
.
├── CMakeLists.txt
├── include
│ └── my_msgs
├── msg
├── package.xml
├── src
└── srv
msg 폴더 안에 아래와 같은 내용의 파일을 작성한다. 확장자는 .msg
int64 num
srv 폴더 안에는 아래와 같은 내용의 파일을 작성한다. 확장자는 .srv
int64 a
int64 b
int64 c
---
int64 sum
--- 를 기준으로 위가 a,b,c가 request , 아래 sum이 response이다.
a, b, c를 리퀘로 받아서 합을 리스폰스로 전달해주는 예제를 작성할 것이다.
이제, CMakeLists.txt를 에 아래와 같은 내용을 추가한다.
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Num.msg"
"srv/AddThreeInts.srv"
)
package.xml 에서는 아래와 같은 내용을 추가한다. 인터페이스가 rosidl_default_generators에 디펜드하기 때문에 디펜던시를 아래와 같이 정의해줘야 한다.
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
배경에서 언급한 rosidl이다 rosidl_generate_interfaces에 방금 만든 인터페이스를 위와 같이 넣어주고 빌드하면 num.hpp 와 같이 language specific한 코드를 생성해주어 코드 내에서 인클루드해 쓸 수 있다. C++ 뿐만 아니라 파이썬 클래스도 생성해주며 워크스페이스 안의 build/my_msgs 폴더에 가보면 rosidl_generator_어쩌구로 생성되어 있는 걸 확인할 수 있다. 이제 아래의 커맨드로 빌드해주자.
colcon build --packages-select my_msgs
아래의 커맨드로 환경셋업을 소스한다.
source ./install/setup.bash
이제 아래 커맨드로 메세지, 서비스가 잘 생성되었는지 확인한다. ros2가 현재 인식하고 있는 인터페이스를 출력해준다.
ros2 interface show my_msgs/msg/Num
#int 64
ros2 interface show my_msgs/srv/AddThreeInts
#int64 a
#int64 b
#int64 c
#---
#int64 sum
이제 이 메세지와 서비스를 다른 패키지에서 활용해보자.
publisher와 subscriber에서의 활용
현재 워크스페이스에서 아래의 명령어로 my_pub_sub, my_srv_cli 패키지를 만들자.
ros2 pkg create --build-type my_pub_sub
my_pub_sub부터 작성해보자.
패키지 내 src폴더에 아래와 같은 파일을 작성한다. 이름은 subscriber.cpp publisher.cpp
subscriber.cpp
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "my_msgs/msg/num.hpp" // CHANGE
using std::placeholders::_1;
class MinimalSubscriber : public rclcpp::Node
{
public:
MinimalSubscriber()
: Node("sub_node")
{
subscription_ = this->create_subscription<my_msgs::msg::Num>( // CHANGE
"topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
}
private:
void topic_callback(const my_msgs::msg::Num::SharedPtr msg) const // CHANGE
{
RCLCPP_INFO(this->get_logger(), "I heard: '%d'", msg->num); // CHANGE
}
rclcpp::Subscription<my_msgs::msg::Num>::SharedPtr subscription_; // CHANGE
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalSubscriber>());
rclcpp::shutdown();
return 0;
}publisher.cpp
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "my_msgs/msg/num.hpp" // CHANGE
using namespace std::chrono_literals;
class MinimalPublisher : public rclcpp::Node
{
public:
MinimalPublisher()
: Node("pub_node"), count_(0)
{
publisher_ = this->create_publisher<my_msgs::msg::Num>("topic", 10); // CHANGE
timer_ = this->create_wall_timer(
500ms, std::bind(&MinimalPublisher::timer_callback, this));
}
private:
void timer_callback()
{
auto message = my_msgs::msg::Num(); // CHANGE
message.num = this->count_++; // CHANGE
RCLCPP_INFO(this->get_logger(), "Publishing: '%d'", message.num); // CHANGE
publisher_->publish(message);
}
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<my_msgs::msg::Num>::SharedPtr publisher_; // CHANGE
size_t count_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalPublisher>());
rclcpp::shutdown();
return 0;
}pub sub은 워낙 쉬운 예제이므로 자세한 설명은 생략한다. 추후 포스팅 하겠다.
커스텀 메세지를 만들고 패키지 내로 불러와서 쓰는 걸 보는 예제이니 인클루드에서 #include "my_msgs/msg/num.hpp" 에 주목한다. 우리가 만든 메세지가 rosidl에 의해 build파일에 잘 들어갔고 코드 내에서 잘 인클루드 되었다. 실행까지 시켜본다.
CMakeLists.txt 와 package.xml에 아래와 같은 내용을 추가한다. 내용은 my_msgs를 이 패키지에서 쓰기 위해 find_package하는 것, 익스큐터블의 타겟 디펜던시를 정의해주는 것. install 에 익스큐터블을 설치하는 것이다.
CMakeLists.txt
...
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(my_msgs REQUIRED)
add_executable(publisher src/publisher.cpp)
ament_target_dependencies(publisher rclcpp my_msgs)
add_executable(subscriber src/subscriber.cpp)
ament_target_dependencies(subscriber rclcpp my_msgs)
install(TARGETS
subscriber
publisher
DESTINATION lib/${PROJECT_NAME})
...package.xml
...
<depend>rclcpp</depend>
<depend>my_msgs</depend>
...
이제 워크스페이스 내에서 아래 명령어로 빌드해준다
colcon build --packages-select my_pub_sub
실행해본다. 실행하기 전에 source ./install/setup.bash를 잊지 말자.
source ./install/setup.bash
ros2 run my_pub_sub subscrber
(커맨드창 새로 열고)
ros2 run my_pub_sub publisher결과
[INFO] [1712490710.176362552] [pub_node]: Publishing: '0'
[INFO] [1712490710.676168864] [pub_node]: Publishing: '1'
[INFO] [1712490711.175931228] [pub_node]: Publishing: '2'
[INFO] [1712490711.675544464] [pub_node]: Publishing: '3'
[INFO] [1712490712.175394549] [pub_node]: Publishing: '4'
[INFO] [1712490712.675035923] [pub_node]: Publishing: '5'
...
...
[INFO] [1712490735.664328195] [sub_node]: I heard: '51'
[INFO] [1712490736.163920192] [sub_node]: I heard: '52'
[INFO] [1712490736.663570998] [sub_node]: I heard: '53'
[INFO] [1712490737.163436327] [sub_node]: I heard: '54'
[INFO] [1712490737.663128343] [sub_node]: I heard: '55'
[INFO] [1712490738.162720868] [sub_node]: I heard: '56'
..
이제 서비스와 클라이언트를 작성해본다.
service와 client에서의 활용
ros2 pkg create --build_type my_srvcli
패키지 내 src폴더에 아래와 같은 파일을 작성한다. 이름은 service.cpp client.cpp
service.cpp
#include "rclcpp/rclcpp.hpp"
#include "my_msgs/srv/add_three_ints.hpp" // CHANGE
#include <memory>
void add(const std::shared_ptr<my_msgs::srv::AddThreeInts::Request> request, // CHANGE
std::shared_ptr<my_msgs::srv::AddThreeInts::Response> response) // CHANGE
{
response->sum = request->a + request->b + request->c; // CHANGE
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld" " c: %ld", // CHANGE
request->a, request->b, request->c); // CHANGE
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_three_ints_server"); // CHANGE
rclcpp::Service<my_msgs::srv::AddThreeInts>::SharedPtr service = // CHANGE
node->create_service<my_msgs::srv::AddThreeInts>("add_three_ints", &add); // CHANGE
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add three ints."); // CHANGE
rclcpp::spin(node);
rclcpp::shutdown();
}
pubsub예제와 마찬가지로 my_msgs가 잘 인클루드 되었고, main함수 안을 보면, Node::make_shared를 통해 나 "add_three_ints_server"노드요~ 하고 선언이 되었다. 이후 rclcpp::Service와 node->create_service를 통해 "add_three_ints"라는 서비스를 받겠다. 선언하고 받았을 때는 add함수를 실행시킨다고 선언한다. 구조를 보면 알겠지만 사용할 때 구조가 subscriber랑 똑같다.(create_subscription + 콜백)
마지막으로 add 함수를 보면 const로 request를 전달받고 포인터를 받아 response->sum에 값을 할당하는 것을 볼 수 있다.
client.cpp
#include "rclcpp/rclcpp.hpp"
#include "my_msgs/srv/add_three_ints.hpp" // CHANGE
#include <chrono>
#include <cstdlib>
#include <memory>
using namespace std::chrono_literals;
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
if (argc != 4) { // CHANGE
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_three_ints_client X Y Z"); // CHANGE
return 1;
}
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_three_ints_client"); // CHANGE
rclcpp::Client<my_msgs::srv::AddThreeInts>::SharedPtr client = // CHANGE
node->create_client<my_msgs::srv::AddThreeInts>("add_three_ints"); // CHANGE
auto request = std::make_shared<my_msgs::srv::AddThreeInts::Request>(); // CHANGE
request->a = atoll(argv[1]);
request->b = atoll(argv[2]);
request->c = atoll(argv[3]); // CHANGE
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::executor::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_three_ints"); // CHANGE
}
rclcpp::shutdown();
return 0;
}커맨드 창에서 아규먼트를 전달할 것이다. argc(입력 아규먼트 개수)가 4개가 제대로 입력되지 않으면 종료, 리턴 1을 한다. node 선언을 하고 create_client를 통해 add three ints라는 이름으로 클라이언트 선언 해준다.
그 다음 리퀘스트에 차례로 입력받은 숫자를 할당한다.
그 다음의 while문은 클라이언트에게 서비스를 기다리게 하는 역할이다. 노드를 켰을 때 1초간 서비스가 잡히지 않으면 while문에 갇혀서 계속 대기하는 것이다. 대기하던 도중에 rclcpp::ok()가 false면 리턴 0을 통해 종료된다. 이 반복문에 갇히지 않으면, 즉 서비스가 정상적으로 잡히면 client->async_send_request(request)를 통해 리퀘를 보내게 된다.
사실 이 예제에서는 바로 리퀘스트 보내버리는데 pub sub이 그러하듯 자신의 용도에 맞게 저 코드 내에 리퀘를 배치하면 된다. 여러개가 될 수도 있다. 예제에 갇히지 말자.
마저 코드를 설명하면 rclcpp::spin_until_future_complete(node, result)를 통해 노드와 result를 전달하여 result가 성공적으로 전달되었는지를 체크한다. 잘 도착했으면 result의 sum, 실패하면 실패 메세지를 출력한다.
이제 아래 내용을 CMakeLists.txt, package.xml에 추가하고 빌드해보자.
CMakeLists.txt
...
find_package(rclcpp REQUIRED)
find_package(my_msgs REQUIRED)
add_executable(service src/service.cpp)
ament_target_dependencies(service rclcpp my_msgs)
add_executable(client src/client.cpp)
ament_target_dependencies(client rclcpp my_msgs)
install(TARGETS
client
service
DESTINATION lib/${PROJECT_NAME})
...별내용 없다. pubsub때와 같이 my_msgs find_package하고 익스큐터블 각각 추가해주면 된다!
package.xml
...
<depend>rclcpp</depend>
<depend>my_msgs</depend>
...
이제 워크스페이스 내에서 아래 명령어로 빌드해준다
colcon build --packages-select my_srvcli
실행해보자.
source ./install/setup.bash
ros2 run my_srvcli service
(커맨드창 새로 열고)
ros2 run my_srvcli client 1 2 3
결과
client
[INFO] [1712576349.573454741] [rclcpp]: Sum: 6
service
[INFO] [1712576325.059741925] [rclcpp]: Ready to add three ints.
[INFO] [1712576349.572891224] [rclcpp]: Incoming request
a: 1 b: 2 c: 3
[INFO] [1712576349.572970801] [rclcpp]: sending back response: [6]
1,2,3을 전달했더니 6을 리스폰스로 잘 전달해줬다. 공홈에 있는 예제이지만 잘 정리해두고 템플릿으로 쓰기 좋다. service client는 그 활용처가 무궁무진하다. 엘레베이터로 다른 층에 도착했다는 플래그가 날라오면 map_update 서비스를 보내 다음 층의 map을 받는다던지, 문고리를 발견하면 상대좌표를 계산해주는 노드에 문고리의 상대좌표를 요청한다던지, 아주 유용하게 쓰일 수 있다.
'로보틱스' 카테고리의 다른 글
| ROS2 cpp 파라미터 선언 및 코드 내 활용(2) - yaml 파일 활용하기 (0) | 2024.03.05 |
|---|---|
| ROS2 cpp 파라미터 선언 및 코드 내 활용(1) (0) | 2024.02.24 |
| ROS2 cpp 패키지 만들기 (0) | 2024.02.19 |
| Ubuntu 20.04(우분투 20.04) ROS2(foxy)설치 (0) | 2024.02.19 |