환경

이 기사는 다음 환경에서 작동합니다.


품목
값


CPU
Core i5-8250U

우분투
16.04

ROS
키네틱

Gazebo
7.14.0


설치에 대한 자세한 내용은 ROS 강좌02 설치을 참조하십시오.
또한이 기사의 프로그램은 github에 업로드되었습니다. ROS 강좌 11 git 저장소을 참조하십시오.

개요

move_base는 파라미터 설정만으로 간단하게 사용할 수 있어 간편합니다만, 아무래도 세부의 동작을 커스텀하고 싶습니다.
Navigation의 동작은 애플리케이션에 따라 다르므로 planner의 교체나 파라미터의 변경만으로는 대응할 수 없습니다.
그래서 여기서는 move_base 상당의 프로그램을 실제로 구현해 봅시다. move_base는 코드가 1000행 정도로 길고 복잡한 일을 하고 있을 것 같습니다만, 플러그인의 처리나 recover 동작을 뺀 한 대로 움직일 뿐인 부분이라면 이하와 같이 100행 정도로 구현할 수 버립니다 .
planner의 어려운 알고리즘은 잘 클래스에 밀려 있기 때문에, 단지 현명하게 생각하고 대답을 돌려주는 함수 정도의 취급으로 보겠습니다.

소스 코드

move_base와 마찬가지로 global_costmap, local_costmap, global_planner, local_planner를 갖지만 recovery_behavior는 없습니다.

외부로부터 PoseStamped형의 토픽을 받으면, 현재 위치로부터 골의 global_plan을 1회만 풀고, 그 후 5Hz로 local_plan을 풀어 갑니다. 이상 등의 처리는 없습니다.

move_base의 기본값에 따라 global_planner는 navfn을 사용하고 local_planner는 base_local_planner를 사용합니다.

nav_lecture/src/move_navigation.cpp

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/trajectory_planner_ros.h>
#include <navfn/navfn_ros.h>

enum class NavState{
  STANDBY,
  WAIT_PLAN,
  MOVING
};

class MoveNavigation {
public:
  MoveNavigation() : global_costmap_("global_costmap", tf_), local_costmap_("local_costmap", tf_) {
    twist_pub_ = nh_.advertise<geometry_msgs::Twist>("/dtw_robot1/diff_drive_controller/cmd_vel", 10);
    goal_sub_ = nh_.subscribe("/move_base_simple/goal", 10, &MoveNavigation::goalCallback, this);
    global_planner_.initialize("global_planner", &global_costmap_);
    local_planner_.initialize("local_planner", &tf_, &local_costmap_);
    nav_state_ = NavState::STANDBY;
    timer_ = nh_.createTimer(ros::Duration(0.2), &MoveNavigation::timerCallback, this);
  }

  void goalCallback(const geometry_msgs::PoseStamped msg){
    ROS_INFO("recieve");
    last_pose_ = msg;
    nav_state_ = NavState::WAIT_PLAN;
  }

  void timerCallback(const ros::TimerEvent& e){
    if (nav_state_ == NavState::WAIT_PLAN){
      ROS_INFO("PLAN");

      geometry_msgs::PoseStamped source_pose;
      source_pose.header.frame_id="dtw_robot1/base_link";
      source_pose.header.stamp=ros::Time(0);
      source_pose.pose.orientation.w=1.0;

      geometry_msgs::PoseStamped target_pose;
      std::string target_frame="dtw_robot1/map";
      try{
        tf_.waitForTransform(source_pose.header.frame_id, target_frame, ros::Time(0), ros::Duration(1.0));
        tf_.transformPose(target_frame, source_pose, target_pose);
        ROS_INFO("x:%+5.2f, y:%+5.2f,z:%+5.2f",target_pose.pose.position.x,target_pose.pose.position.y,target_pose.pose.position.z);
      }
      catch(...){
        ROS_INFO("tf error");
      }
      geometry_msgs::PoseStamped start = target_pose;

      if (!global_planner_.makePlan(start, last_pose_, last_global_plan_)){
        ROS_WARN("global plan fail");
        nav_state_ = NavState::STANDBY;
        return;
      }
      local_planner_.setPlan(last_global_plan_);
      geometry_msgs::Twist cmd_vel;
      if (local_planner_.isGoalReached()){
        ROS_INFO("reach");
        twist_pub_.publish(cmd_vel);
        nav_state_ = NavState::STANDBY;
        return;
      }
      local_planner_.computeVelocityCommands(cmd_vel);
      twist_pub_.publish(cmd_vel);
      nav_state_ = NavState::MOVING;
    }
    else if(nav_state_ == NavState::MOVING){
      ROS_INFO_THROTTLE(2.0, "MOVING");
      geometry_msgs::Twist cmd_vel;
      if (local_planner_.isGoalReached()){
        ROS_INFO("reach");
        twist_pub_.publish(cmd_vel);
        nav_state_ = NavState::STANDBY;
        return;
      }
      local_planner_.computeVelocityCommands(cmd_vel);
      twist_pub_.publish(cmd_vel);
    }
  }
  ros::NodeHandle nh_;
  tf::TransformListener tf_;
  ros::Publisher twist_pub_;
  ros::Subscriber goal_sub_;
  ros::Timer timer_;

  NavState nav_state_;
  geometry_msgs::PoseStamped last_pose_;
  std::vector<geometry_msgs::PoseStamped> last_global_plan_;

  costmap_2d::Costmap2DROS global_costmap_;
  costmap_2d::Costmap2DROS local_costmap_;
  navfn::NavfnROS global_planner_;
  base_local_planner::TrajectoryPlannerROS local_planner_;
};

int main(int argc, char** argv){
  ros::init(argc, argv, "move_navigation");
  MoveNavigation move_navigatoion;
  ros::spin();
  return 0;
}

노드의 움직임을 NavState로 관리하고 있습니다.

기동 직후는 STANDBY

goal이 설정되면 WAIT_PLAN

위의 첫 번째 timer_loop에서 global_plan이 수행되고 local_plan이 실행되는 동안 MOVING

local_plan이 끝나면 STANDBY로 돌아갑니다

global_plan에는 시작점과 종료점이 필요합니다. 끝점은 외부에서 주어지지만 시작점은 스스로 결정해야합니다. 대체로는 지금의 현재 위치를 넣으면 좋을 것입니다.

makePlan() 로 시작점과 끝점을 연결하는 점열을 얻습니다.

local_plan 에서는 최초로 global plan 의 출력의 점열을 세트 해, 그 후는 일정 주기로 함수를 호출해 속도 지시치를 얻습니다.

setPlan() 에서 global plan의 출력을 설정합니다.

computeVelocityCommands() 로 현재 속도 지표 값을 얻습니다.

isGoalReached() 로 골에 도착했는지 결정합니다.

빌드

cd ~/catkin_ws
catkin_make

실행

실행

roslaunch nav_lecture move_navigation.launch 

Rviz의 2DNavGoal을 사용하여 PoseStamped 주제를 던지면 거기로 이동합니다.

참고

move_base

목차 페이지 링크

ROS 강좌의 목차에 대한 링크