
13. [Option] C++ ์ฝ๋, template - 2
์ง๋ ์๊ฐ์ ์ด์ด์ roscpp์ฝ๋๋ค์ ๊ณ์ ์ดํด๋ณด๊ฒ ์ต๋๋ค.
spawn_model_client.cpp
/*
* gazebo model spawn by rosservice
*
* referenced from answers.ros.org
* url : https://pastebin.com/UTWJSScZ
*
* basic template of roscpp service client
*
* referenced from wiki.ros.org
* url : http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29
*/
#include <fstream> // ros.h doesn't contain this lib
#include <ros/ros.h>
#include <ros/package.h>
#include <gazebo_msgs/SpawnModel.h>
#include <geometry_msgs/Pose.h>
void addXml(gazebo_msgs::SpawnModel& model_in, const std::string& file_path ){
std::ifstream file(file_path);
std::string line;
while (!file.eof()){
std::getline(file, line);
model_in.request.model_xml += line;
}
file.close();
}
geometry_msgs::Pose getPose(){
geometry_msgs::Pose initial_pose;
initial_pose.position.x = -2;
initial_pose.position.y = 1;
initial_pose.position.z = 1;
initial_pose.orientation.z = -0.707;
initial_pose.orientation.w = 0.707;
return initial_pose;
}
int main(int argc, char** argv){
ros::init(argc, argv, "gazebo_spawn_model");
ros::NodeHandle nh;
ros::ServiceClient spawn_model_prox = nh.serviceClient<gazebo_msgs::SpawnModel>("gazebo/spawn_urdf_model");
gazebo_msgs::SpawnModel model;
// add roslib in find_package()
auto file_path = ros::package::getPath("cpp_service_tutorial") + "/models/r2d2.urdf";
addXml(model, file_path);
model.request.model_name = "r2d2";
model.request.reference_frame = "world";
model.request.initial_pose = getPose();
// ServiceClient.call() => return bool type
if (spawn_model_prox.call(model)){
auto response = model.response;
ROS_INFO("%s", response.status_message.c_str()); // Print the result given by the service called
}
else {
ROS_ERROR("Failed to call service /trajectory_by_name");
return 1;
}
return 0;
}
์ด๋ฒ ์ฝ๋์์๋ ๋ณธ๋์ ๋ชฉ์ ์ธ serviceClient
๋ฅผ ์์ฑํ๋ ๊ฒ๊ณผ ๋๋ถ์ด
์ฌ๋ฌ ์ ํธ๋ฆฌํฐ๋ค์ ์ดํด๋ณด๊ณ ์ ํฉ๋๋ค.
์ฐ์ Service Client
์ ์์ฑ์
๋๋ค.
ros::ServiceClient spawn_model_prox = nh.serviceClient<gazebo_msgs::SpawnModel>("gazebo/spawn_urdf_model");
์ฃผ๊ณ ๋ฐ์ srv๋ก instantiationํด์ฃผ๊ณ , Server์ ์ด๋ฆ์ ๋งค๊ฐ๋ณ์๋ก ๋ฃ์ด์ฃผ์์ต๋๋ค.
ํ์ด์ฌ๊ณผ ์ฐจ์ด๊ฐ ๋๋ ๋ถ๋ถ์ด ์ฌ๊ธฐ ์๋๋ฐ์,
gazebo_msgs::SpawnModel ๊ตฌ์กฐ์ฒด ์์, request
,
response
๊ฐ ๊ฐ์ด ๋ค์ด์๊ธฐ์ ์๋์ ๊ฐ์ด ์ฌ์ฉ๋ฉ๋๋ค.
gazebo_msgs::SpawnModel model;
model.request.model_name = "r2d2";
model.request.reference_frame = "world";
model.request.initial_pose = getPose();
// ServiceClient.call() => return bool type
if (spawn_model_prox.call(model)){
auto response = model.response;
ROS_INFO("%s", response.status_message.c_str()); // Print the result given by the service called
}
urdf
ํ์ผ ์ ์ถ๋ ฅ ๋ถ๋ถ์ ๋ณผ๊น์?
#include <fstream> // ros.h doesn't contain this lib
...
void addXml(gazebo_msgs::SpawnModel& model_in, const std::string& file_path ){
std::ifstream file(file_path);
std::string line;
while (!file.eof()){
std::getline(file, line);
model_in.request.model_xml += line;
}
file.close();
}
ros.h์๋ fstream์ด ์์ต๋๋ค.
๊ทธ๋์ ๋ฐ๋ก include ํด์ฃผ์๊ณ , ์ ์ฝ๋๋ ifstream
์ผ๋ก๋ถํฐ std::string
์ ํ์ค์ฉ ๋ด์ model_xml
์๊ฒ ๋๊ฒจ์ฃผ๋ ๋ถ๋ถ์
๋๋ค. ์ด ์ฝ๋๋ ๋ค๋ฅธ ๊ณณ์์๋ ์ฌ์ฌ์ฉ์ด ๊ฐ๋ฅํ ๊ฒ์
๋๋ค.
- ๋ค์์ผ๋ก service call์ ํ๋ ๋ถ๋ถ์ ๋๋ค.
// ServiceClient.call() => return bool type
if (spawn_model_prox.call(model)){
auto response = model.response;
ROS_INFO("%s", response.status_message.c_str()); // Print the result given by the service called
}
else {
ROS_ERROR("Failed to call service /trajectory_by_name");
return 1;
}
call()
ํจ์๊ฐ bool type์ return ํ๊ธฐ ๋๋ฌธ์ ์์ ๊ฐ์ ๋ก์ง์ด
๊ตฌํ๋์์ต๋๋ค.
๊ธฐ์กด ํ์ด์ฌ์์๋ print๋ง ํ๋ฉด ์์์ ์์๊ฒ ์ถ๋ ฅํด์ฃผ์๋๋ฐ C++์
c_str()
๋ฑ์ ์ฒ๋ฆฌ๊ฐ ๋ค์ด๊ฐ๋๋ค.
- ์ถ๊ฐ์ ์ผ๋ก, ํ์ผ ๊ฒฝ๋ก๋ฅผ ๊ฐ์ ธ์ค๋ ์ํฉ์์ ์ ์ฉํ
ros::package::getPath
์ ๋๋ค.
#include <ros/package.h>
...
// add roslib in find_package()
auto file_path = ros::package::getPath("cpp_service_tutorial") + "/models/r2d2.urdf";
์ด์ ํ์ด์ฌ Service ์์ ์๋ ๋ก๋ด์ ํ์ ์ํค๋ ์ฝ๋๋ฅผ ์ดํด๋ณด๊ฒ ์ต๋๋ค.
robot_turning_client.cpp
/*
* turn your robot with angular velocity
*
* created by kimsooyoung : https://github.com/kimsooyoung
*/
#include <ros/ros.h>
#include "cpp_service_tutorial/ControlTurningMessage.h"
using srv_t = cpp_service_tutorial::ControlTurningMessage;
using std::cout;
using std::endl;
using std::cin;
int main(int argc, char** argv) {
ros::init(argc, argv, "robot_turning_client");
ros::NodeHandle nh;
ros::ServiceClient client = nh.serviceClient<srv_t>("/control_robot_angle");
ROS_INFO("==== Service Client initialized ====");
// srv type
// uint32 time_duration
// float64 angular_vel
// ---
// bool success
srv_t srv;
unsigned int time_duration;
float angular_vel;
cout << "Enter time_duration : ";
cin >> time_duration;
cin.clear(); cin.ignore(32767, '\n');
cout << "Enter angular_vel : ";
cin >> angular_vel;
cin.clear(); cin.ignore(32767, '\n');
srv.request.time_duration = time_duration;
srv.request.angular_vel = angular_vel;
if ( client.call(srv) ){
auto response = srv.response;
cout << std::boolalpha;
cout << "Response : " << bool(response.success) << endl;
}
else {
ROS_ERROR("Failed to call service /control_robot_angle");
return 1;
}
return 0;
}
์ด๋์ ๋ ์๊ฐ ๋์ ์ผ๋งํผ์ ๊ฐ์๋๋ก ํ์ ํ ๊ฒ์ธ์ง, ์ด๋ฅผ ์ํด ์ฐ๋ฆฌ๋ง์ custom srv๋ฅผ
๋ง๋ค์์ต๋๋ค. ์ด๋ ๊ฒ ๋ง๋ค์ด์ง srv๋ "ํจํค์ง ์ด๋ฆ"/".srv ํ์ผ ์ด๋ฆ"
์ผ๋ก
include๊ฐ ๊ฐ๋ฅํฉ๋๋ค.
๊ทธ๋ฌ๋ค๋ณด๋ ์ด๋ฆ์ด ๋๋ฌด ๊ธธ์ด์ ธ์ ์ ๋ ์ถ์ฝํ์ ๋ฐ๋ก ์ ์ํ์์ต๋๋ค.
using srv_t = cpp_service_tutorial::ControlTurningMessage srv_t;
- service client ์์ฑ๊ณผ call์ ํ๋ ๋ถ๋ถ์ ๋๋ค. ์์ gazebo ์์ ์์ ์ดํด๋ณด์๊ธฐ์ ๊ฐ๋จํ ๋์ด๊ฐ๋๋ค.
ros::ServiceClient client = nh.serviceClient<srv_t>("/control_robot_angle");
(...)
if ( client.call(srv) ){
- ์ฌ์ฉ์๋ก๋ถํฐ input์ ๋ฐ๋ ๋ถ๋ถ์ด ์์ต๋๋ค. ๋ฐ๋ก ์์ธ์ฒ๋ฆฌ๋ ํ์ง ์์์ง๋ง, ์ค์ ํ๋ก์ ํธ๋ฅผ ์งํํ๋ฉด์๋ ๋๋ฌด ํฐ ๊ฐ์๋๋ฅผ ๊ฐ์ง ์๋๋ก ํด์ผ ํ ๊ฒ์ ๋๋ค. ์ปค๋ค๋ ๋ก๋ด์ด ๊ฐ์๊ธฐ ์์ฒญ๋ ํ์ ์ ํ๋ค๋ฉด.... ์๊ฐ๋ง ํด๋ ๋์ฐํฉ๋๋ค. ๐ค๐ค
cout << "Enter time_duration : ";
cin >> time_duration;
cin.ignore(32767, '\n'); cin.clear();
cout << "Enter angular_vel : ";
cin >> angular_vel;
cin.ignore(32767, '\n'); cin.clear();
robot_turning_srv.cpp
/*
* referenced from answer.ros.org
*
* url : https://answers.ros.org/question/214597/service-with-class-method/
*/
#include <chrono>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include "cpp_service_tutorial/ControlTurningMessage.h"
using Request_T = cpp_service_tutorial::ControlTurningMessage::Request;
using Response_T = cpp_service_tutorial::ControlTurningMessage::Response;
class TinyBot {
private:
ros::NodeHandle m_nh;
ros::ServiceServer m_control_ss; // control service_server
ros::Publisher m_vel_pub;
geometry_msgs::Twist m_cmd_vel;
std::string m_name;
public:
TinyBot(const std::string &name_in = "tinybot"): m_name(name_in) {
m_control_ss = m_nh.advertiseService("/control_robot_angle", &TinyBot::servCallback, this);
m_vel_pub = m_nh.advertise<geometry_msgs::Twist>("/cmd_vel", 5);
ROS_INFO("Service Server and Publisher initialized");
ROS_INFO("Waiting for request...");
}
/*
* srv type
*
* uint32 time_duration
* float64 angular_vel
* ---
* bool success
*/
bool servCallback(Request_T &req, Response_T &res) {
m_cmd_vel.angular.z = req.angular_vel;
ROS_INFO("==== Start Turning ====");
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() < req.time_duration){
m_vel_pub.publish(m_cmd_vel);
now = std::chrono::steady_clock::now();
time_duration = now - start;
}
// while ( ros::Time::now() < end_time ){
// // std::cout << ros::Time::now() << std::endl;
// m_vel_pub.publish(m_cmd_vel);
// }
ROS_WARN("==== Stop Turning ====");
m_cmd_vel.angular.z = 0;
m_vel_pub.publish(m_cmd_vel);
res.success = true;
return 1;
}
};
int main(int argc, char** argv){
ros::init(argc, argv, "robot_turning_server");
auto myTinyBot = TinyBot();
ros::spin();
return 0;
}
- ControlTurningMessage.srv
uint32 time_duration
float64 angular_vel
---
bool success
ํด๋์ค๋ฅผ ์ฌ์ฉํ๊ณ ์์ต๋๋ค. ๊ทธ ์ด์ ์ ๋ํด์๋ ์์ ์ด์ผ๊ธฐํ ๋ฐ์ ๊ฐ์ต๋๋ค.
- ์ฃผ๋ ๋ด์ฉ์ธ service server๋ฅผ ์์ฑํ๊ณ ์๋ ๋ชจ์ต์ ๋๋ค. ๋ฉค๋ฒ ํจ์๋ฅผ ์ฝ๋ฐฑ์ผ๋ก ์ฌ์ฉํ๊ธฐ ์ํด์, function pointer, object๋ฅผ ๋ฐ๋ ๊ฒ์ด ๋ณด์ ๋๋ค.
TinyBot(const std::string &name_in = "tinybot"): m_name(name_in) {
m_control_ss = m_nh.advertiseService("/control_robot_angle", &TinyBot::servCallback, this);
m_vel_pub = m_nh.advertise<geometry_msgs::Twist>("/cmd_vel", 5);
ROS_INFO("Service Server and Publisher initialized");
ROS_INFO("Waiting for request...");
}
์ด์ ๋ ํด๋์ค ํํ๋ก ๊ตฌํํ๋ ๊ฒ์ด ์กฐ๊ธ์ ์ต์ํด์ง์
จ๋์? ๐ ์ฃผ๋ก ์ฌ์ฉ๋๋
/cmd_vel
, /scan
๊ณผ ๊ฐ์ publisher, subscriber๋ฅผ ๋ด์
ํด๋์ค๋ฅผ ๋ฐ๋ก ๋ง๋ค์ด ์ฌ์ฌ์ฉํ ์๋ ์๊ฒ ์ง์??
๋์ค์ ์ค์ ๋ก๋ด์ ์ ์ดํ์ ๋ค๋ฉด , ๊ฑฐ์ ROS์์ ๊ตฌํ๋ ๊ธฐ๋ฅ๋ค์ ์์๋ฐ์ ์ฌ์ฉํ์๊ฒ ๋ฉ๋๋ค. ์ด๋ฅผ ์ํด์๋ผ๋ ํด๋์ค๋ก์ ๊ตฌํ์ ์ต์ํด์ง์ จ์ผ๋ฉด ์ข๊ฒ ์ต๋๋ค!!
- service server callback
// srv type
// uint32 time_duration
// float64 angular_vel
// ---
// bool success
bool servCallback(Request_T &req, Response_T &res) {
m_cmd_vel.angular.z = req.angular_vel;
while (time_duration.count() < req.time_duration){
...
res.success = true;
...
}
๊ธฐ์กด callback๋ค๊ณผ ๋ค๋ฅด๊ฒ, request, response ๋๊ฐ์ ๋งค๊ฐ๋ณ์๋ฅผ ๊ฐ์ต๋๋ค.
๊ธฐ๋ฅ์์ ๋ก์ง์ ์๋ตํ๊ณ , ์ด์ Action์ผ๋ก ๋์ด๊ฐ๋๋ก ํ๊ฒ ์ต๋๋ค.
fibonacci_action_server.cpp
์์ํ๊ธฐ์ ์์, action์ ์ฌ์ฉํ๊ธฐ ์ํด์๋ roscpp ๋ง๊ณ ๋ ๋ค์๊ณผ ๊ฐ์ ํจํค์ง๋ค์
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
actionlib_msgs
actionlib # client requires both packages
roscpp
std_msgs
)
python์์๋ ์ด๋ ๊ฒ ์ฐจ์ด๊ฐ ์์์ง์?
import rospy
import actionlib
from actionlib_tutorials.msg import FibonacciAction, FibonacciGoal
client = actionlib.SimpleActionClient("fibonacci_action_server", FibonacciAction)
- ๋๋ถ์ด ํผ๋ณด๋์น ์์ ๋ฅผ ์์ ํ๋ฉด์ ํ์ด์ฌ์์๋ sequence๋ฅผ list๋ก ๋ค๋ฃจ์๋๋ฐ์. C++์์๋ std::vector๋ก ๋ค๋ฃจ๊ฒ ๋ฉ๋๋ค.
Fibonacci.action
#goal definition
int32 order
---
#result definition
int32[] sequence
---
#feedback
int32[] sequence
/*
* referenced from wiki.ros.org
* url : http://wiki.ros.org/actionlib_tutorials/Tutorials/SimpleActionServer%28ExecuteCallbackMethod%29
*/
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/FibonacciAction.h>
/* Fibonacci.action
*
* goal definition
* int32 order
* ---
* result definition
* int32[] sequence
* ---
* feedback
* int32[] sequence
*/
class FibonacciActionClass {
protected:
ros::NodeHandle m_nh;
actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> m_as;
std::string m_name;
// vector
actionlib_tutorials::FibonacciFeedback m_feedback;
actionlib_tutorials::FibonacciResult m_result;
public:
FibonacciActionClass(const std::string &name_in = "fibonacci_action_server"):
m_name(name_in),
m_as(m_nh, name_in, boost::bind(&FibonacciActionClass::actionCallback, this, _1), false) {
m_as.start();
ROS_INFO("==== Action Server Class Constructed ====");
}
~FibonacciActionClass(){
ROS_INFO("==== Action Server Class Destroying ====");
}
// must use Ptr type or ConstPtr type!!
void actionCallback(const actionlib_tutorials::FibonacciGoalConstPtr &goal){
ros::Rate r(5);
bool success = true;
int order = int(goal->order);
const auto sequence = &(m_feedback.sequence);
// push_back the seeds for the fibonacci sequence
sequence->clear();
sequence->push_back(0);
sequence->push_back(1);
// publish info to the console for the user
std::cout << m_name.c_str() << ": Executing, creating fibonacci sequence of order " <<
order << " with seeds " << sequence->at(0) << " , " << sequence->at(1) << std::endl;
// start executing the action
for(int i = 1; i <= order; i++){
// check that preempt has not been requested by the client
if (m_as.isPreemptRequested() || !ros::ok()){
ROS_INFO("%s: Preempted", m_name.c_str());
// set the action state to preempted
m_as.setPreempted();
success = false;
break;
}
sequence->push_back(sequence->at(i) + sequence->at(i-1));
// publish the feedback
m_as.publishFeedback(m_feedback);
// this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep();
}
if(success){
m_result.sequence = m_feedback.sequence;
ROS_INFO("%s: Succeeded", m_name.c_str());
// set the action state to succeeded
m_as.setSucceeded(m_result);
}
}
};
int main(int argc, char** argv){
ros::init(argc, argv, "fibonacci");
FibonacciActionClass fibonacci;
ros::spin();
return 0;
}
- import ์์ ๋ค์๊ณผ ๊ฐ์ ์ฐจ์ด์ ์ด ์๋ค๋ ์ ์ ์ ์ํฉ๋๋ค.
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/FibonacciAction.h>
- ์์ฑ์ ๋ถ๋ถ์ ๋ณด๊ฒ ์ต๋๋ค.
class FibonacciActionClass {
protected:
ros::NodeHandle m_nh;
actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> m_as;
std::string m_name;
// vector
actionlib_tutorials::FibonacciFeedback m_feedback;
actionlib_tutorials::FibonacciResult m_result;
public:
FibonacciActionClass(const std::string &name_in = "fibonacci_action_server"):
m_name(name_in),
m_as(m_nh, name_in, boost::bind(&FibonacciActionClass::actionCallback, this, _1), false) {
m_as.start();
ROS_INFO("==== Action Server Class Constructed ====");
}
~FibonacciActionClass(){
ROS_INFO("==== Action Server Class Destroying ====");
}
- action server๋ ์๋์ ๊ฐ์ด template instantiation์ ์ฌ์ฉํ์ฌ ์์ฑํฉ๋๋ค. ๋๋ถ์ด server์ด๊ธฐ ๋๋ฌธ์, feedback๊ณผ result๊ฐ ์ฌ์ฉ๋๊ณ ์์ต๋๋ค.
actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> m_as;
std::string m_name;
// vector
actionlib_tutorials::FibonacciFeedback m_feedback;
actionlib_tutorials::FibonacciResult m_result;
- ~๊ฐ ๋ถ์ ๋ถ๋ถ์ destructor, ์๋ฉธ์์ ๋๋ค. ๊ทธ๋ฐ๋ฐ, Action Server์ goal callback์ ์ง์ ํด์ฃผ๋ ๋ถ๋ถ์ ์๋ก์ด ๋ฌธ๋ฒ์ด ๋ณด์ ๋๋ค.
FibonacciActionClass(const std::string &name_in = "fibonacci_action_server"):
m_name(name_in),
m_as(m_nh, name_in, boost::bind(&FibonacciActionClass::actionCallback, this, _1), false) {
m_as.start();
ROS_INFO("==== Action Server Class Constructed ====");
}
~FibonacciActionClass(){
ROS_INFO("==== Action Server Class Destroying ====");
}
boost::bind(&FibonacciActionClass::actionCallback, this, _1)
์ ๋ํ ๋ง์ ์ง์์ ๊ฐ์ง ๊ฒ์ด ์๋๋ผ ๊ฐ๋จํ๊ฒ ๋ค์ด๊ฐ๋ฉด,
actionCallback
์ด ๋ช๊ฐ์ ์ธ์๋ฅผ ๋ฐ์์ง,
๊ทธ ์ธ์๋ค์ด ์ด๋ค ํ์
์ผ์ง, ํจ์์ผ์ง, ํฌ์ธํฐ์ผ์ง, ๋ชจ๋ฅด๋ ์ํฉ์์ ์ฝ๋ฐฑ์ ์คํ์ ๋ณด๋ค ํธ๋ฆฌํ๊ฒ ํ๊ธฐ ์ํด, ์ฝ๋์ ์์ ์ค์ด๊ธฐ ์ํด ์ฌ์ฉ๋์๋ค๊ณ ์ดํดํ์๋ฉด ์ข์
๊ฒ ๊ฐ์ต๋๋ค.
์ง๊ธ์ ๊ฒฝ์ฐ function pointer์ object, ๊ทธ๋ฆฌ๊ณ placeholder ํ๋๊ฐ bind ๋์์ต๋๋ค.
- callback์, ์ ์ ์ฌํญ์ด ์์ต๋๋ค. goal์ ์ ๋ฌ๋ฐ์ ๋, ๋ฐ๋์ Ptr type์ผ๋ก ์ ๋ฌ๋ฐ์์ผ ํฉ๋๋ค!
void actionCallback(const actionlib_tutorials::FibonacciGoalConstPtr &goal){
or
void actionCallback(const actionlib_tutorials::FibonacciGoalPtr &goal){
- ์ดํ์ ๊ณผ์ ์ ๋น์ทํ๊ณ , publishFeedback, setSucceeded์ ํตํด feedback/result๋ฅผ ๋ณด๋ธ๋ค๋ ์ ,
ROS_INFO๋ฅผ ์ฌ์ฉํ๋ ๋ฐฉ๋ฒ์ ๋ํ ์ถ๊ฐ ์ค๋ช ์ ๋๋ฆด ์ ์์ต๋๋ค.
if (m_as.isPreemptRequested() || !ros::ok()){
ROS_INFO("%s: Preempted", m_name.c_str());
// set the action state to preempted
m_as.setPreempted();
success = false;
break;
}
sequence->push_back(sequence->at(i) + sequence->at(i-1));
// publish the feedback
m_as.publishFeedback(m_feedback);
// this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep();
}
if(success){
m_result.sequence = m_feedback.sequence;
ROS_INFO("%s: Succeeded", m_name.c_str());
// set the action state to succeeded
m_as.setSucceeded(m_result);
}
fibonacci_action_client.cpp
/*
* referenced from wiki.ros.org
* url : http://wiki.ros.org/actionlib_tutorials/Tutorials/Writing%20a%20Callback%20Based%20Simple%20Action%20Client
*/
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib_tutorials/FibonacciAction.h>
/* Fibonacci.action
*
* goal definition
* int32 order
* ---
* result definition
* int32[] sequence
* ---
* feedback
* int32[] sequence
*/
void doneCb(const actionlib::SimpleClientGoalState& state,
const actionlib_tutorials::FibonacciResultConstPtr& result)
{
ROS_INFO("[State Result]: %s", state.toString().c_str());
ROS_INFO("Answer: %i", result->sequence.back());
ros::shutdown();
}
// Called once when the goal becomes active
void activeCb()
{
ROS_INFO("Goal just went active");
}
void feedbackCb(const actionlib_tutorials::FibonacciFeedbackConstPtr& feedback){
ROS_INFO("[Feedback] :");
for(auto &elem : feedback->sequence){
std::cout << elem << " ";
}
std::cout << std::endl;
}
int main(int argv, char** argc){
ros::init(argv, argc, "fibonacci_client");
ros::NodeHandle nh;
ros::Rate r(5);
std::string server_name = "fibonacci_action_server";
// turn on auto threading
actionlib::SimpleActionClient<actionlib_tutorials::FibonacciAction> client(server_name, true);
std::cout << "Wating for Server..." << std::endl;
client.waitForServer();
actionlib_tutorials::FibonacciGoal goal;
goal.order = 20;
std::cout << "==== Sending Goal to Server ====" << std::endl;
client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
actionlib::SimpleClientGoalState state_result = client.getState();
while (!state_result.isDone()){
// Doing Stuff while waiting for the Server to give a result....
// if (<cancel-condition>){
// client.cancelGoal();
// }
state_result = client.getState();
r.sleep();
}
if (state_result == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("[Action Done State Result]: %s", state_result.toString().c_str());
else
ROS_ERROR("[Something Gonna Wrong State Result]: %s", state_result.toString().c_str());
return 0;
}
- action client๋ ๋ค์๊ณผ ๊ฐ์ด ์์ฑํฉ๋๋ค. ๋๋ฒ์งธ ์ธ์๋ auto_threading์ผ๋ก
ros::spin()
์ ์๋์ผ๋ก ์คํ์์ผ์ฃผ๋ ์ต์ ์ ๋๋ค.

actionlib::SimpleActionClient<actionlib_tutorials::FibonacciAction> client(server_name, true);
sendGoal
๋ถ๋ถ์ ์ดํด๋ณด๊ฒ ๋๋ฐ์, ์ฝ๋ฐฑ์ด ๋ค์ ๋ง์ต๋๋ค ๐ ์์๋๋ก- ์๋ฃ ์ ์คํ๋๋ done_callback
- ์ด๊ธฐ active ์ ํ๋ฒ๋ง ์คํ๋๋ active_callback
- ์๋ฒ๋ก๋ถํฐ feedback์ ๋ฐ์ ๋๋ง๋ค ์คํ๋๋ feedback_callback
์ด๋ ๊ฒ ์ธ๊ฐ์ง ํ์์ด ์์ต๋๋ค.
๊ฐ callback์ผ๋ก ๋์์ค๋ ์ธ์๋ค์ ์ง์คํด์ ์ดํด๋ณด๊ฒ ์ต๋๋ค.
void doneCb(const actionlib::SimpleClientGoalState& state,
const actionlib_tutorials::FibonacciResultConstPtr& result)
{
ROS_INFO("[State Result]: %s", state.toString().c_str());
ROS_INFO("Answer: %i", result->sequence.back());
ros::shutdown();
}
// Called once when the goal becomes active
void activeCb()
{
ROS_INFO("Goal just went active");
}
void feedbackCb(const actionlib_tutorials::FibonacciFeedbackConstPtr& feedback){
ROS_INFO("[Feedback] :");
for(auto &elem : feedback->sequence){
std::cout << elem << " ";
}
std::cout << std::endl;
}
client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
- Action Client๋ server๊ฐ ์์ ์ ์ํ์ค์ธ ๋์ ๋ค๋ฅธ์ผ์ ํ ์ ์๋ค๊ณ ๋งํ์ง์?
๋๋ถ์ด, goal cancel์ ์์ฒญํ ์ ์๋ cancelGoal()
ํจ์๋
๋ณด์
๋๋ค.
while (!state_result.isDone()){
// Doing Stuff while waiting for the Server to give a result....
// if (<cancel-condition>){
// client.cancelGoal();
// }
state_result = client.getState();
r.sleep();
}
- ์ต์ข result state์ ๋ฐ๋ผ ๋๋ฒ๊น ๋ฉ์ธ์ง๋ฅผ ์ถ๋ ฅํ๋ ๋ถ๋ถ์ ๋๋ค.
if (state_result == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("[Action Done State Result]: %s", state_result.toString().c_str());
else
ROS_ERROR("[Something Gonna Wrong State Result]: %s", state_result.toString().c_str());
์์ ํ์ด์ฌ ํ๋ก๊ทธ๋๋ฐ์ ๋ฐฐ์ ๊ธฐ์, ๋์๋๋ ํจ์๋ค์ ํ๋ฒ์ฉ ์ดํด๋ณด์์ต๋๋ค.
๊ทธ๋ผ ์ด๋ ค์ด C++์ ์ ์ฐ๋์ โโ
๋ก๋ด์ ๊ฒฐ๊ตญ ํ๋์จ์ด๋ฅผ ๋ค๋ฃจ๋ ์์ ์ ๋๋ค. ๊ฐ๋จํ ์ด์ผ๊ธฐํด์, ์ ํ์ ๋ง๋ค๊ฑฐ๋ ์ฐ๊ตฌ๋ฅผ ํ๋ ์ํฉ์์, ์ฌ์ฉํ๋ ํ๋์จ์ด, ์๊ณ ๋ฆฌ์ฆ์ผ๋ก์ ์ต์ ํ๊ฐ ํ์ํ๊ธฐ์ C++์ ์ฌ์ฉํ๋ค๊ณ ๋ง์๋๋ฆฌ๊ณ ์ถ์ต๋๋ค.
์ต๊ณ ์ ์ฑ๋ฅ์ด ํ์ํ์ง ์๊ณ , ๋น ๋ฅด๊ฒ ๊ฐ๋ฐํ์ฌ ๊ฒฐ๊ณผ๋ฅผ ๋ณด๊ณ ์ถ์ ์ฐ๋ฆฌ์ ๊ฒฝ์ฐ, ํ์ด์ฌ์ด ๋ ์ ํฉํ๋ค๊ณ ์๊ฐํฉ๋๋ค. ๊ทธ๋์ ์์ผ๋ก์ ROS RC Car ํ๋ก์ ํธ์์๋ ํ์ด์ฌ์ ์ฌ์ฉํ๊ตฌ์.
Reference
์ฝ์ด๋ณผ๋งํ ๋ ํผ๋ฐ์ค๋ค์ ๋งํฌํด ๋์์ต๋๋ค.
๊ณ ๊ธ ROS ํ๋ก๊ทธ๋๋จธ๋ ๊ฑฐ๋ญ๋๊ธฐ ์ํด ์ ๋ ์ด์ฌํ ๊ณต๋ถ์ค์ด๋๋๋ค. ๐


๋ค์ ์๊ฐ์๋rviz
,rqt
์ ๊ฐ์ ๋๋ฒ๊น ์์ฐ๊ตฌ ๋์์ฃผ๋ ์ ์ฉํ ํด๋ค์ ๋ํด์ ๋ฐฐ์๋ณด๋๋ก ํ๊ฒ ์ต๋๋ค!!