[KUSMO] 9/22 Map 좌표 지정을 통한 이동 명령

 

• 로봇을 SLAM을 통해 작성한 Map 상에서 원하는 지점으로 이동시키는 코드

/*
    1 = Recharging Area (Default)
    2 = Sickroom
    3 = Exhausting Area
*/

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <iostream>

using namespace std;

// Action specification for move_base
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

int main(int argc, char** argv){
    // Connect to ROS
    ros::init(argc, argv, "simple_navigation_goals");

    //tell the action client that we want to spin a thread by default
    MoveBaseClient ac("move_base", true);

    // Wait for the action server to come up so that we can begin processing goals.
    while(!ac.waitForServer(ros::Duration(5.0))){
    // ROS_INFO("move_base action 서버 대기 중");
    cout << "move_base action 서버 대기 중" << endl;
    }

    int user_choice = 1;
    char choice_to_continue = 'Y';
    bool run = true;

    while(run) {

    // Ask the user where he wants the robot to go?
    cout << "\n목표 지점을 입력하세요" << endl;
    cout << "\n1 = 충전 구역" << endl;
    cout << "2 = 병실 1" << endl;
    cout << "3 = 폐기 구역" << endl;
    cout << "\n숫자를 입력하세요: ";
    cin >> user_choice;

    // Create a new goal to send to move_base 
    move_base_msgs::MoveBaseGoal goal;

    // Send a goal to the robot
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();

    bool valid_selection = true;

    // Use map_server to load the map of the environment on the /map topic. 
    // Launch RViz and click the Publish Point button in RViz to 
    // display the coordinates to the /clicked_point topic.
    switch (user_choice) {
        case 1:
        cout << "\n목표 지점 : 충전 구역\n" << endl;
        goal.target_pose.pose.position.x = -0.217960834503;
        goal.target_pose.pose.position.y = -1.04363012314;
        goal.target_pose.pose.position.z = 0.0;
        goal.target_pose.pose.orientation.x = 0.0;
        goal.target_pose.pose.orientation.y = 0.0;
        goal.target_pose.pose.orientation.z = 0.678093828428;
        goal.target_pose.pose.orientation.w = 0.734975346422;
        break;
        case 2:
        cout << "\n목표 지점 : 병실\n" << endl;
        goal.target_pose.pose.position.x = 2.5;
        goal.target_pose.pose.position.y = 0.15;
        goal.target_pose.pose.position.z = 0.0;
        goal.target_pose.pose.orientation.x = 0.0;
        goal.target_pose.pose.orientation.y = 0.0;
        goal.target_pose.pose.orientation.z =  0.027398240287736; 
        goal.target_pose.pose.orientation.w = 0.999624597751144;
        break;
        case 3:
        cout << "\n목표 지점 : 폐기 구역\n" << endl;
        goal.target_pose.pose.position.x = 0.314909696579;
        goal.target_pose.pose.position.y = 1.12254846096;
        goal.target_pose.pose.position.z = 0.0;
        goal.target_pose.pose.orientation.x = 0.0;
        goal.target_pose.pose.orientation.y = 0.0;
        goal.target_pose.pose.orientation.z = 0.41090931658603;
        goal.target_pose.pose.orientation.w = 0.999155411002427;
        break;
        default:
        cout << "\n유효하지 않은 값입니다. 다시 입력해주세요.\n" << endl;
        valid_selection = false;
    }       

    // Go back to beginning if the selection is invalid.
    if(!valid_selection) {
        continue;
    }

    ROS_INFO("Sending goal");
    ac.sendGoal(goal);

    // Wait until the robot reaches the goal
    ac.waitForResult();

    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        // ROS_INFO("로봇이 목표 지점에 도달했습니다");
        cout << "로봇이 목표 지점에 도달했습니다" << endl;
    else
        // ROS_INFO("로봇이 목표지점에 도달하지 못했습니다");
        cout << "로봇이 목표지점에 도달하지 못했습니다" << endl;

    // Ask the user if he wants to continue giving goals
    do {
        cout << "\n계속 진행하시겠습니까? (Y/N)" << endl;
        cin >> choice_to_continue;
      choice_to_continue = tolower(choice_to_continue); // Put your letter to its lower case
        } 
    while (choice_to_continue != 'n' && choice_to_continue != 'y'); 

    if(choice_to_continue =='n') {
        run = false;
        }  
    }

    return 0;
}

 

- 코드 설명

    // Connect to ROS
    ros::init(argc, argv, "simple_navigation_goals");

• ROS node명을 simple_navigation_goals로 초기화하여 통신을 위한 ROS 환경 설정

 

    //tell the action client that we want to spin a thread by default
    MoveBaseClient ac("move_base", true);

• move_base action server와 상호작용하기 위해 ac라는 이름의 MoveBaseCilent Class의 Instance 생성
  • move_base action server는 robot의 움직임을 제어하는 역할을 함

 

    // Wait for the action server to come up so that we can begin processing goals.
    while(!ac.waitForServer(ros::Duration(5.0))){
    // ROS_INFO("move_base action 서버 대기 중");
    cout << "move_base action 서버 대기 중" << endl;
    }

• move_base action server가 사용 가능해질때 까지 대기하는 while loop문
• 5초까지 server가 나오기를 기다림

 

    while(run) {

    // Ask the user where he wants the robot to go?
    cout << "\n목표 지점을 입력하세요" << endl;
    cout << "\n1 = 충전 구역" << endl;
    cout << "2 = 병실 1" << endl;
    cout << "3 = 폐기 구역" << endl;
    cout << "\n숫자를 입력하세요: ";
    cin >> user_choice;

    // Create a new goal to send to move_base 
    move_base_msgs::MoveBaseGoal goal;

    // Send a goal to the robot
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();

    bool valid_selection = true;

    // Use map_server to load the map of the environment on the /map topic. 
    // Launch RViz and click the Publish Point button in RViz to 
    // display the coordinates to the /clicked_point topic.
    switch (user_choice) {
        case 1:
        cout << "\n목표 지점 : 충전 구역\n" << endl;
        goal.target_pose.pose.position.x = -0.217960834503;
        goal.target_pose.pose.position.y = -1.04363012314;
        goal.target_pose.pose.position.z = 0.0;
        goal.target_pose.pose.orientation.x = 0.0;
        goal.target_pose.pose.orientation.y = 0.0;
        goal.target_pose.pose.orientation.z = 0.678093828428;
        goal.target_pose.pose.orientation.w = 0.734975346422;
        break;
        case 2:
        cout << "\n목표 지점 : 병실\n" << endl;
        goal.target_pose.pose.position.x = 2.5;
        goal.target_pose.pose.position.y = 0.15;
        goal.target_pose.pose.position.z = 0.0;
        goal.target_pose.pose.orientation.x = 0.0;
        goal.target_pose.pose.orientation.y = 0.0;
        goal.target_pose.pose.orientation.z =  0.027398240287736;  // -0.0364083684981
        goal.target_pose.pose.orientation.w = 0.999624597751144;
        break;
        case 3:
        cout << "\n목표 지점 : 폐기 구역\n" << endl;
        goal.target_pose.pose.position.x = 0.314909696579;
        goal.target_pose.pose.position.y = 1.12254846096;
        goal.target_pose.pose.position.z = 0.0;
        goal.target_pose.pose.orientation.x = 0.0;
        goal.target_pose.pose.orientation.y = 0.0;
        goal.target_pose.pose.orientation.z = 0.41090931658603;
        goal.target_pose.pose.orientation.w = 0.999155411002427;
        break;
        default:
        cout << "\n유효하지 않은 값입니다. 다시 입력해주세요.\n" << endl;
        valid_selection = false;
    }

• 사용자로부터 원하는 목표지점을 숫자를 통해 입력받고, 그에 해당하는 특정 goal이 생성됨
  • goal은 desired position과 orientation 정보와 함께 생성됨
• Position x, y, z 값 은 Rviz 상에서 '2D Nav Goal' 버튼을 통해 목표로 하는 위치로 이동하게 하고, 이를 통해 발행되는 goal_2d topic을 통해 구함

 

• Orientation x, y, z, w는 위에서 구한  아래의 Euler Angle을 Quaternion으로 변환하는 공식을 통해 계산

 

    ROS_INFO("Sending goal");
    ac.sendGoal(goal);

• sendGoal 함수를 통해 move_base action server로 goal을 보냄
• 이후 move_base action server는 robot이 goal 까지 이동하기 위한 경로를 생성하고 실행하는 역할을 함

 

    // Wait until the robot reaches the goal
    ac.waitForResult();

    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        // ROS_INFO("로봇이 목표 지점에 도달했습니다");
        cout << "로봇이 목표 지점에 도달했습니다" << endl;
    else
        // ROS_INFO("로봇이 목표지점에 도달하지 못했습니다");
        cout << "로봇이 목표지점에 도달하지 못했습니다" << endl;

• Robot이 goal에 도착하기를 기다림
• Robot이 목표지점에 정상적으로 도착했는지 여부를 확인하고 각 여부에 따른 메세지를 출력

 

    // Ask the user if he wants to continue giving goals
    do {
        cout << "\n계속 진행하시겠습니까? (Y/N)" << endl;
        cin >> choice_to_continue;
      choice_to_continue = tolower(choice_to_continue); // Put your letter to its lower case
        } 
    while (choice_to_continue != 'n' && choice_to_continue != 'y'); 

    if(choice_to_continue =='n') {
        run = false;
        }  
    }

• Robot의 목표 지점 도착 여부를 확인한 후, do while문을 통해 위의 과정을 반복할 것인지를 확인
• 이후 tolower 함수를 통해 입력받은 문자를 소문자로 변형
• 만약 Y, y, N, n에 해당하지 않는 문자를 입력할 경우 맨 위의 do 부분으로 이동하여 입력을 다시하도록 함
• 변형된 문자가 'n'일 경우 Flag 역할을 하는 변수 run의 값을 false로 변경하여 위 과정을 반복하는 while 문을 빠져나오도록 함

 

 

 

 

---

참고 자료 :

https://automaticaddison.com/how-to-send-goals-to-the-ros-navigation-stack-using-c/

How To Send Goals to the ROS Navigation Stack Using C++ – Automatic Addison