μΉ΄ν…Œκ³ λ¦¬ μ—†μŒ

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

Swimming_Kim 2021. 1. 29. 18:57
🧐

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와 같은 디버깅을연ꡬ λ„μ™€μ£ΌλŠ” μœ μš©ν•œ νˆ΄λ“€μ— λŒ€ν•΄μ„œ λ°°μ›Œλ³΄λ„λ‘ ν•˜κ² μŠ΅λ‹ˆλ‹€!!