지난 시간에 이어서 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 에게 넘겨주는 부부입니다. 이 코드는 다른 곳에서도 재사용이 가능할 것입니다.