Study/[ROS2] ROS2 Basic
ROS2 with C++ - Service
soohwan_justin
2023. 7. 30. 19:06
본 포스트는 ROS1에 대한 전반적인 내용 및 ROS2 Python에 대한 지식이 있다는 가정 하에 작성되었으며, The Construct Youtube 영상을 참고하였습니다.
또한, 시뮬레이션 환경은 turtlebot3 waffle_pi를 사용합니다 : https://github.com/ROBOTIS-GIT/turtlebot3_simulations
1. Service Client
기본적인 내용은 python과 같으니, 다 생략하고 바로 예제로 넘어갑니다.
1. 패키지 만들기
$ ros2 pkg create client_cpp_pkg --build-type ament_cmake --dependencies rclcpp std_srvs
2. cpp 코드 작성
service_client.cpp
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/empty.hpp"
#include <chrono>
#include <cstdlib>
#include <memory>
using namespace std::chrono_literals;
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("service_client");
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
node->create_client<std_srvs::srv::Empty>("reset_world");
auto request = std::make_shared<std_srvs::srv::Empty::Request>();
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_future = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(node, result_future) ==
rclcpp::FutureReturnCode::SUCCESS)
{
auto result = result_future.get();
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "The world has been reset");
} else {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service /reset_world");
}
rclcpp::shutdown();
return 0;
}
3. launch 파일 만들기
service_client.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='client_cpp_pkg',
executable='service_client_node',
output='screen'),
])
4. CMakeLists.txt 수정
cmake_minimum_required(VERSION 3.8)
project(client_cpp_pkg)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_srvs REQUIRED)
add_executable(service_client_node src/service_client.cpp)
ament_target_dependencies(service_client_node rclcpp std_srvs)
install(TARGETS
service_client_node
DESTINATION lib/${PROJECT_NAME}
)
# Install launch files.
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}/
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
5. 빌드 후, 실행
$ ros2 launch client_pkg service_client_launch_file.launch.py
launch 파일을 실행하면 시뮬레이션이 초기화되는 것을 볼 수 있습니다
2. Spinning Node에서 service call 하기
이전 예제의 line 29~31을 보면 다음과 같습니다.
그런데 node가 이미 디폴트로 spin 상태로 선언되어있을 수 있습니다. 이런 경우에 대한 예제를 보겠습니다.
1. cpp파일 생성
service_client_2.cpp
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "std_srvs/srv/empty.hpp"
#include <chrono>
#include <cstdlib>
#include <future>
#include <memory>
using namespace std::chrono_literals;
class ServiceClient : public rclcpp::Node {
private:
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client_;
rclcpp::TimerBase::SharedPtr timer_;
bool service_done_ = false;
void timer_callback() {
while (!client_->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(
this->get_logger(),
"Client interrupted while waiting for service. Terminating...");
return;
}
RCLCPP_INFO(this->get_logger(),
"Service Unavailable. Waiting for Service...");
}
auto request = std::make_shared<std_srvs::srv::Empty::Request>();
service_done_ = false;
auto result_future = client_->async_send_request(
request, std::bind(&ServiceClient::response_callback, this,
std::placeholders::_1));
}
void
response_callback(rclcpp::Client<std_srvs::srv::Empty>::SharedFuture future) {
auto status = future.wait_for(1s);
if (status == std::future_status::ready) {
RCLCPP_INFO(this->get_logger(), "Result: success");
service_done_ = true;
} else {
RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
}
}
public:
ServiceClient() : Node("service_client") {
client_ = this->create_client<std_srvs::srv::Empty>("reset_world");
timer_ = this->create_wall_timer(
1s, std::bind(&ServiceClient::timer_callback, this));
}
bool is_service_done() const { return this->service_done_; }
};
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
auto service_client = std::make_shared<ServiceClient>();
while (!service_client->is_service_done()) {
rclcpp::spin_some(service_client);
}
rclcpp::shutdown();
return 0;
}
2. launch 파일 생성, CMakeLists.txt 파일 수정 후 실행해봅니다.
결과는 이전과 같습니다. 이전 코드와 달라진 부분은 아래와 같습니다.
3. Service Server 만들기
1. 패키지 생성 후, 코드 작성
$ ros2 pkg create server_cpp_pkg --build-type ament_cmake --dependencies rclcpp geometry_msgs std_srvs
service_server.cpp
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/set_bool.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <memory>
using SetBool = std_srvs::srv::SetBool;
using std::placeholders::_1;
using std::placeholders::_2;
class ServerNode : public rclcpp::Node
{
public:
ServerNode()
: Node("service_moving")
{
srv_ = create_service<SetBool>("moving_right", std::bind(&ServerNode::moving_callback, this, _1, _2));
publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
}
private:
rclcpp::Service<SetBool>::SharedPtr srv_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
void moving_callback(
const std::shared_ptr<SetBool::Request> request,
const std::shared_ptr<SetBool::Response> response)
{
auto message = geometry_msgs::msg::Twist();
if (request->data == true)
{
message.linear.x = 0.2;
message.angular.z = -0.2;
publisher_->publish(message);
response->success = true;
response->message = "Turning to the right!";
}
if (request->data == false)
{
message.linear.x = 0.0;
message.angular.z = 0.0;
publisher_->publish(message);
response->success = false;
response->message = "stop!";
}
}
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<ServerNode>());
rclcpp::shutdown();
return 0;
}
2. launch 파일 작성
service_server.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='server_cpp_pkg',
executable='service_server_node',
output='screen'),
])
3. CMakeLists.txt 수정
CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(server_cpp_pkg)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_srvs REQUIRED)
add_executable(service_server_node src/service_server.cpp)
ament_target_dependencies(service_server_node rclcpp geometry_msgs std_srvs)
install(TARGETS
service_server_node
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}/
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
4. 빌드 및 실행
$ ros2 service call /moving_right std_srvs/srv/SetBool data:\ true\
$ ros2 service call /moving_right std_srvs/srv/SetBool data:\ false\
Custom srvice만드는 법은 python과 동일하므로 생략하겠습니다.