13. [Option] C++ μ½λ, template - 2

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
μ κ°μ λλ²κΉ μμ°κ΅¬ λμμ£Όλ μ μ©ν ν΄λ€μ λν΄μ λ°°μ보λλ‘ νκ² μ΅λλ€!!