๋ณธ๋ฌธ ๋ฐ”๋กœ๊ฐ€๊ธฐ

์นดํ…Œ๊ณ ๋ฆฌ ์—†์Œ

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()์„ ์ž๋™์œผ๋กœ ์‹คํ–‰์‹œ์ผœ์ฃผ๋Š” ์˜ต์…˜์ž…๋‹ˆ๋‹ค.
Wiki
Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. Description: This tutorial covers using the simple_action_client library to create a averaging action client.
http://wiki.ros.org/actionlib_tutorials/Tutorials/SimpleActionClient%28Threaded%29
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์™€ ๊ฐ™์€ ๋””๋ฒ„๊น…์„์—ฐ๊ตฌ ๋„์™€์ฃผ๋Š” ์œ ์šฉํ•œ ํˆด๋“ค์— ๋Œ€ํ•ด์„œ ๋ฐฐ์›Œ๋ณด๋„๋ก ํ•˜๊ฒ ์Šต๋‹ˆ๋‹ค!!