전 과정에서 rviz의 2D Nav Goal 버튼을 이용해서 로봇에게 목표 지점을 주고 움직여보도록 하였습니다. 그럼 이제 사용자가 만든 노드에서 로봇에게 명령을 내리려면 어떻게 해야 할까요?

토픽 (/move_base_simple/goal) 으로 명령을 줘도 되지만, 이 경우 로봇이 잘 움직이고 있는지, 골 지점에 도착했는지에 대한 것을 확인이 어렵습니다.

/move_base_simple/goal (geometry_msgs/PoseStamped.msg)

PoseStamped.msg

	# A Pose with reference coordinate frame and timestamp
	Header header
	Pose pose

이때 사용해야 하는 것이 Actionlib입니다. move_base는 MoveBase.action 타입의 action 서버를 제공합니다. 따라서 이를 이용하면 노드에서 로봇에 명령을 줄 수 있고, 현재 동작 및 골에 도착했는지에 대한 확인이 가능합니다.

이제 navigation client 패키지를 만들어봅시다.

$ catkin create pkg mobile_manipulator_robot_navigation_client --catkin-deps move_base_msgs tf2 actionlib actionlib_msgs roscpp

src 디렉토리 아래에 nav_client_node.cpp를 만들고 아래와 같이 입력합니다.

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <move_base_msgs/MoveBaseAction.h>
#include "tf2/LinearMath/Transform.h"
#include "tf2/utils.h"

void callback_done(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result)
{
    ROS_INFO("Robot reached to goal.");
}

void callback_active()
{
    ROS_INFO("Robot start move.");
}

void callback_feedback(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback)
{
    ROS_INFO("Current robot pose is %f %f %f on /map",
                        feedback->base_position.pose.position.x, feedback->base_position.pose.position.y,
                        tf2::getYaw(feedback->base_position.pose.orientation) * 180 / 3.141592654);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "nav_client_node");

    if (argc != 4)
    {
        ROS_INFO("usage: nav_client_node <X> <Y> <THETA(deg)>");
        return 1;
    }

    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base", true);

    ROS_INFO("Waiting for action server to start.");
    ac.waitForServer();
    ROS_INFO("Action server started, sending goal.");

    move_base_msgs::MoveBaseGoal goal;
    // goal.goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.pose.position.x = atof(argv[1]);
    goal.target_pose.pose.position.y = atof(argv[2]);
    goal.target_pose.pose.position.z = 0;

    tf2::Quaternion q;
    q.setRPY(0, 0, atof(argv[2]) * 3.141592654 / 180.0 );

    goal.target_pose.pose.orientation.x = q[0];
    goal.target_pose.pose.orientation.y = q[1];
    goal.target_pose.pose.orientation.z = q[2];
    goal.target_pose.pose.orientation.w = q[3];

    ac.sendGoal(goal, &callback_done, &callback_active, &callback_feedback);

    ros::spin();
    return 0;
}

코드를 하나씩 살펴보면,

#include <move_base_msgs/MoveBaseAction.h>
#include "tf2/LinearMath/Transform.h"
#include "tf2/utils.h"

MoveBase action에 대한 헤더 파일과 tf2 헤더 파일을 include 합니다. (Quaternion 계산에 필요)

actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base", true);

SimpleActionClient를 생성해 줍니다. action 이름은 "move_base"