이타인클럽

코스 전체목록

닫기

Waypoint Navigation

지정된 waypoints로 로봇을 움직여 봅니다.

토픽 결과물: Mobile 로봇이 정해진 points을 순차적으로 이동하게 됩니다.

 

Mobile 로봇 Simulation 모듈에서는 wall following 주행에 대한 내용을 다뤘는데, 이번에는 waypoints, 즉 목표점들로 순차적으로 주행하게 하는 방법에 대해서 알아봅니다. 여기서는 Mobile 로봇 Simulation 모듈의 코드 기반으로 하되 #if __WAYPOINT_NAV__ #endif 구문을 이용하여 waypoint navigation을 위한 실행코드와 비실행 코드로 구분하였습니다. 또한, Mobile 로봇 Simulation 모듈의 코드는 coverage 관련기능을 사용하고, waypoint navigation을 위해서 별도의 ROS noded와 launch 파일을 생성합니다. 또한, 장애물 회피 및 위치 인식을 위해 다음과 같은 ROS 패키지를 사용합니다.

  • gmapping: 위치 인식
  • move_base: 경로 계획, 장애물 회피, 로봇 주행 제어 등

설명 동영상

CMakeLists.txt 추가 

waypoints를 yaml 파일에 저장하고 사용할 것이기 때문에 아래와 같이 yaml-cpp를 추가합니다.

find_package(PkgConfig)
pkg_search_module(yaml-cpp REQUIRED yaml-cpp)

그 다음 코드상에서 __WAYPOINT_NAV__로 실행코드와 주석코드 설정을 위해 아래와 같이 추가합니다.

## set waypoint navigation
ADD_DEFINITIONS( -D__WAYPOINT_NAV__=1 )

waypoint navigation을 위한 ROS node를 추가합니다. 노드 이름은 waypoint_nav_node 입니다. linking 라이브러리에 yaml-cpp가 사용되고 있습니다.

## waypoint nav node
add_executable( waypoint_nav_node src/waypoint_nav.cpp )

## Specify libraries to link a library or executable target against
target_link_libraries( waypoint_nav_node
  ${catkin_LIBRARIES}
  yaml-cpp
)

mobile_robot_sim.cpp 수정

Mobile 로봇 Simulation 모듈에서 구현된 소스 코드를 아래와 같이 수정, 추가합니다. 먼저 map frame을 odom frame과 동일하게 tf 출력하는 부분을 비실행처리 합니다. 이것은 map frame이 gmapping 노드에서 생성되기 때문입니다. 그 다음으로 Mobile 로봇 Simulation 모듈의 코드에서는 로봇이 wall following 주행하므로 관련 코드를 주석처리합니다.

void MRS::run()
{
  int queue_size= 1000; //50;

  // subscribe the scan message. use remap tag for the scan topic name
  scan_sub_= ros_nh_.subscribe( "scan", queue_size, &MRS::scanCB, this );

  ...

#if !__WAYPOINT_NAV__
  // run the publishing thread; 30Hz
  publish_thread_= new boost::thread( boost::bind( &MRS::publishTransform, this ) );
#endif
  // coverage map publisher
  coverage_map_pub_= ros_nh_.advertise<nav_msgs::OccupancyGrid>( "coverage_map", 1, true );
}

void MRS::scanCB( const sensor_msgs::LaserScanConstPtr& _scan )
{
  ...

#if !__WAYPOINT_NAV__
  // determine obstacle state
  bool detected= determineObsState( ranges );

  ...
  default:
    ROS_WARN( "Driving state is undefined" );
    break;
  } // end of switch( driving_state_ )
#endif  // end of #if !__WAYPOINT_NAV__
  
  // publish map in case that either no map generated or update time reached
  if( !got_map_ || (_scan->header.stamp - map_updated_time ) > map_update_interval )
  {
    // update the coverage map
    updateCoverageMap( gt_pose_ );
    // publish the coverage map message
    coverage_map_pub_.publish( coverage_map_ );
    // update the time of map updating
    map_updated_time= _scan->header.stamp;
  }
}

turtlebot_hokuyo.launch 추가

Mobile 로봇 Simulation 모듈에서 사용된 launch 파일을 기반으로 새롭게 waypoint_nav argument를 생성하여 실행부분과 비실행 부분을 구분합니다. gmapping 패키지나 move_base 패키지에서도 scan topic이 필요하여 argument를 추가하고 각 패키지에 argument로 전달합니다. gmapping, move_base 패키지는 /opt/ros/indigo/share 밑에 있는 패키지 및 파일을 그대로 사용합니다. 이것은 두 패키지는 사용자가 수정할 것이 별로 없기 때문입니다. 그러나 move_base의 장애물 인식 및 경로 계획과 관련하여 몇 가지 파라미터는 변경해야 합니다. waypoint navigation을 위해 별도의 launch 파일을 만들고, move_base 노드 실행을 하게 하여, gmapping과 move_base의 실행 시간 차이를 두게 합니다. 시간 차이를 두지 않았을 때, local costmap 생성이 제대로 안됩니다. 따라서, turtlebot_hokuyo.launch 파일에서는 gmapping 노드만 실행시키고, move_base와 waypoint_nav 노드는 waypoint_nav.launch 파일에서 실행시킵니다.

<launch>
  <!-- waypoint navigation option -->
  <arg name="waypoint_nav" value="true" />
  
  <!-- scan topic name -->
  <arg name="scan_topic" default="/hokuyo_scan" />
  ...

  <!-- include mobile robot sim launch file -->   
  <include file="$(find mobile_robot_sim)/launch/includes/mrs.launch.xml"> 
    <arg name="waypoint_nav" value="$(arg waypoint_nav)"/>
    <arg name="scan_topic" value="$(arg scan_topic)" />
    <arg name="init_x"     value="$(arg init_x)"/>
    <arg name="init_y"     value="$(arg init_y)"/>
    <arg name="init_theta" value="$(arg init_theta)"/> 
  </include>

  <!-- gmapping -->
  <group if="$(arg waypoint_nav)" >    
    <!-- gmapping -->
    <include file="$(find turtlebot_navigation)/launch/includes/gmapping.launch.xml">
      <arg name="scan_topic" value="$(arg scan_topic)" /> 
    </include>
  </group>
</launch>

mrs.launch.xml 수정

Mobile 로봇 Simulation 모듈에서 사용된 launch 파일을 아래와 같이 수정합니다. 여기서는 scan topic 이름을 mobile_robot_sim에서 요구하는 이름으로 remapping 하는 부분이 추가가 되었습니다. 또한 필요에 따라 waypoint_nav 인자값에 따라 실행 부분과 비실행 부분으로 나뉘게 할 수 있습니다.

<launch>  
  <arg name="waypoint_nav" default="false" />
  <arg name="scan_topic" default="/scan" />
  ...
  <!-- mobile robot sim node -->  
  <node pkg="mobile_robot_sim" type="mobile_robot_sim_node" name="mobile_robot_sim_node" output="screen" args="-init_x $(arg init_x) -init_y $(arg init_y) -init_theta $(arg init_theta)">
    <remap from="scan" to="$(arg scan_topic)"/> 

    ...

  </node>
</launch>

waypoint_nav.cpp 구현

waypoint navigation을 위한 소스 파일을 아래와 같이 구현합니다. 구현은 ROS의 sending simple goals 부분을 참고하여 구현되었습니다. waypoints가 적힌 yaml파일을 읽어서 목표점 하나씩 move_base 패키지에 전달하여 해당 목표점까지 로봇이 이동하게 됩니다.

Include files

//////////// ros related
// ros
#include <ros/ros.h>
// ros package to access package directory
#include <ros/package.h>
// move base
#include <move_base_msgs/MoveBaseAction.h>
// simple move action
#include <actionlib/client/simple_action_client.h>
// stamped point message
#include <geometry_msgs/PointStamped.h>
// tf2 matrix
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
// yaml file handling
#include <yaml-cpp/yaml.h>
//
#include <fstream>

struct

// typedef
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

// parameter struct and typedef
struct WaypointNavParam
{
  // waypoint filename
  std::string waypoints_filename;
  // reference frame. the robot wil move based on this frame
  std::string ref_frame;
  // number of runs
  int num_runs;

};

template<typename T>
void operator >> ( const YAML::Node& node, T& i )
{
    i= node.as<T>();
}

Waypoint Navigation 클래스

/*
 * Waypoint Navigation class
 */
class WaypointNav
{
public:
  ///
  WaypointNav();
  ///
  ~WaypointNav();
  ///
  void init();
  ///
  void run();
  ///
  bool buildWaypointsFromFile();

private:
  /// ros node handle
  ros::NodeHandle ros_nh_;
  ///
  WaypointNavParam params_;
  /// input image file name including the path
  std::string waypoints_filename_;
  /// array of waypoint
  std::vector<geometry_msgs::PointStamped> waypoints_;
};

생성자, 소멸자

/*
 * Constructor
 */
WaypointNav::WaypointNav()
{
}

/*
 * Destructor
 */
WaypointNav::~WaypointNav()
{
}

init() 함수 생성

/*
 * Initialize parameters
 */
void WaypointNav::init()
{
  // private node handle for getting parameters
  ros::NodeHandle ros_nh("~");

  // number of runs to repeat the waypoint navigation
  ros_nh.param<int>( "num_loops", params_.num_runs, 1 );

  //// setup waypoints filename
  // input waypoint filename
  ros_nh.param<std::string>( "waypoints_filename", params_.waypoints_filename, "launch/waypoints/waypoints.yaml" );
  // referece frame
  ros_nh.param<std::string>( "ref_frame", params_.ref_frame, "base_link" );
  // get the package path
  std::string pkg_path= ros::package::getPath( "mobile_robot_sim" );
  waypoints_filename_= pkg_path + "/" + params_.waypoints_filename;
#if __DEBUG__
  ROS_INFO( "number of runs: %d", params_.num_runs );
  ROS_INFO( "waypoints filename: %s", waypoints_filename_.c_str() );
#endif
}

waypoint list 만드는 함수 생성

/*
 * Build waypoints from file
 */
bool WaypointNav::buildWaypointsFromFile()
{
  // clear way points vector
  waypoints_.clear();
  try
  {
    // check file open
    std::ifstream ifs( waypoints_filename_.c_str(), std::ifstream::in );
    if( !ifs.good() )
    {
      return false;
    }
    // yaml node
    YAML::Node yaml_node;
    yaml_node= YAML::Load(ifs);
    const YAML::Node &wp_node_tmp= yaml_node[ "waypoints" ];
    const YAML::Node *wp_node= wp_node_tmp ? &wp_node_tmp : NULL;

    if(wp_node != NULL)
    {
      // loop over all the waypoints
      for(int i=0; i < wp_node->size(); i++)
      {
        // get each waypoint
        geometry_msgs::PointStamped point;
        (*wp_node)[i]["point"]["x"] >> point.point.x;
        (*wp_node)[i]["point"]["y"] >> point.point.y;
        (*wp_node)[i]["point"]["th"] >> point.point.z;
        waypoints_.push_back(point);
      }
    }
    else
    {
      return false;
    }
  }
  catch(YAML::ParserException &e)
  {
      return false;
  }
  catch(YAML::RepresentationException &e)
  {
      return false;
  }

  return true;
}

run() 함수 생성

/*
 * Run waypoint navigation.
 */
void WaypointNav::run()
{
  bool built= buildWaypointsFromFile();
  if( !built )
  {
    ROS_FATAL( "building waypoints from a file failed" );
    return;
  }
  // tell the action client that we want to spin a thread by default
  MoveBaseClient action_client( "move_base", true );

  // wait for the action server to come up
  while( !action_client.waitForServer( ros::Duration(5.0) ) )
  {
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  for( int i=0; i<params_.num_runs; i++ )
  {
    for( int j=0; j<waypoints_.size(); j++ )
    {
      move_base_msgs::MoveBaseGoal goal;
      // send a goal to the robot
      goal.target_pose.header.frame_id= params_.ref_frame;
      goal.target_pose.header.stamp= ros::Time::now();
      goal.target_pose.pose.position.x= waypoints_[j].point.x;
      goal.target_pose.pose.position.y= waypoints_[j].point.y;
      goal.target_pose.pose.position.z= 0.0;

      // convert the degrees to quaternion
      double yaw= waypoints_[j].point.z*M_PI/180.;
      tf2::Quaternion q;
      q.setRPY( 0., 0., yaw );
      goal.target_pose.pose.orientation.x= q.x();
      goal.target_pose.pose.orientation.y= q.y();
      goal.target_pose.pose.orientation.z= q.z();
      goal.target_pose.pose.orientation.w= q.w();

      ROS_INFO("Sending goal: (%.2f, %.2f, %.2f)", goal.target_pose.pose.position.x, goal.target_pose.pose.position.y, goal.target_pose.pose.orientation.z );
      action_client.sendGoal( goal );
      action_client.waitForResult();
      if(action_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        ROS_INFO("The base moved");
      else
        ROS_INFO("The base failed to move");
      ros::Duration(0.5).sleep();
    }
  }
}

main 함수

/*
 * main function
 */
int main( int argc, char **argv )
{
  // ros init
  ros::init( argc, argv, "waypoint_nav_node" );

  // create a waypoint navigation instance
  WaypointNav nav;
  // initialize waypoint navigation
  nav.init();
  // ros run
  nav.run();

  // spin ROS
  try
  {
    // ros takes all
    ros::spin();
  }
  catch( std::runtime_error& e )
  {
    ROS_ERROR( "ros spin failed: %s", e.what() );
    return -1;
  }

  return 0;
}

waypoint_nav.launch 생성

move_base와 waypoint navigation 실행을 담당하는 launch 파일을 새롭게 만듭니다. launch 파일을 분리한 이유는 gmapping과 move_base의 실행 시간 차이를 두게 하여 local costmap이 제대로 그려지게 하기 위함입니다.

<!-- waypoint navigation -->
<launch>
  <!-- scan topic name -->
  <arg name="scan_topic" default="/hokuyo_scan" />
  <arg name="num_runs"     default="1"/>

  <!-- Move base -->
  <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"> 
    <arg name="laser_topic" value="$(arg scan_topic)" />
  </include>     

  <!-- waypoint navigation -->
  <node pkg="mobile_robot_sim" type="waypoint_nav_node" name="waypoint_nav_node" output="screen" >
    <param name="waypoints_filename" value="launch/waypoints/waypoints.yaml" />
    <param name="ref_frame" value="map" />
    <param name="num_runs"    value="$(arg num_runs)"/>
  </node>
</launch>

waypoints.yaml 생성

아래와 같이 로봇이 방문해야 할 지점과 자세를 하나의 point로 하여 기입합니다. 이 때, 시작 위치는 로봇이 놓여 있는 pose가 (0, 0, 0)이다. 아래 예에서는 로봇이 (0,0,0) -> (2.0, 1.0, 0.0 ) -> (2.3, 2.0, 90) -> (0.5, 2.0, 180.0) 으로 이동하도록 설정하였습니다. Point의 기준 좌표계는 파라미터 ref_frame으로 정하게 되는데, 여기서는 map을 했기 때문에 map frame에 대한 Point 값입니다. 만약 로봇 위치 상대적인 값으로 Point을 설정하려면, ref_frame을 base_footprint으로 하면 됩니다. 파일 이름을 패키지 밑의 launch/waypoints/waypoints.yaml로 하였습니다.

# note that the initial pose is (0, 0, 0)
waypoints:
  - point: #1
      x  : 2.0
      y  : 1.0
      th : 0.0 # degrees
  - point: #2
      x  : 2.3
      y  : 2.0
      th : 90.0      
  - point: #3
      x  : 0.5
      y  : 2.0
      th : 180.0

costmap_common_params.yaml

/opt/ros/indigo/share/turtlebot_navigation/param 폴더 밑에 있는 costmap_common_params.yaml 파일을 필요하다면 수정합니다. 주요 파라미터는 아래와 같습니다.

  • obstacle_range: 0.2 #2.5 -> 로봇과 장애물간의 거리가 이 거리 이내라면 장애물로 처리. 작게 할수록 좁은 영역 통과가 가능하다.
  • scan # scan 부분의 파라미터이다. bump 부분이 아님을 주의한다.
  • min_obstacle_height: 0.17 #0.25 -> 장애물 센서의 센싱 최소 높이. 센서가 부착된 높이를 기준으로 해야 한다.
  • max_obstacle_height: 0.27 #0.35 -> 장애물 센서의 센싱 최고 높이.
  • inflation layer
  • inflation_radius: 0.2 # 0.5 -> 장애물 주위 얼마까지를 장애물 cost를 할당할지 결정. 작게 할수록 좁은 영역 통과가 가능하다.

노드 실행

먼저 catkin_make를 하여 소스를 빌드후 아래와 같이 별도의 terminal에 각각의 노드를 실행합니다.

# terminal 1
$ roslaunch mobile_robot_sim turtlebot_hokuyo.launch
# terminal 2
$ roslaunch mobile_robot_sim rviz.launch
# terminal 3
$ roslaunch mobile_robot_sim waypoint_nav.launch

Waypoint 이동 실패시

패키지의 urdf 디렉터리 밑의 turtlebot_gazebo.urdf.xacro 파일에서 아래와 같은 부분에서 max 값을 5.0 정도로 변경한 후 다시 실행합니다. max range가 짧으면 max range 부근은 장애물로 인식하여 waypoint 까지의 경로 생성이 안될 수 있습니다. 따라서 max range를 크게 하여 waypoint 주변에 장애물 생성이 안되도록 합니다.

<!-- Hokuyo LIDAR for simulation -->
  <xacro:macro name="hokuyo_laser">
  ...    
       <range>
            <min>0.01</min>
            <max>5.0</max>

댓글

댓글 본문
graphittie 자세히 보기