ROS Navigation Stack について3 ~ Goalを送信するノードの作成 ~

move_baseとodometryに関しては整ったので,ここではmove_baseに対してゴールを送信するノードを作成します.
引用:http://wiki.ros.org/ja/navigation/Tutorials/SendingSimpleGoals

ただ,今回作成したシステムでは,オドメトリ情報がでたらめなので,実際にロボットでmove_baseを動かすには,ここを調整してあげる必要があります.
実際のodometryノードに関しては,あとのポストで詳しく書ければと思います.

1. Goal送信ノード用のパッケージ作成

Goal送信ノード用のパッケージを生成します.ここでは,simple_goal_generatorパッケージとします.

cd ~/reps/catkin_ws/src
catkin_create_pkg simple_goal_generator move_base_msgs actionlib roscpp

2. Goal送信ノードの作成

ここでは,チュートリアル通りのコードsrc/simple_goal_generator.cppを作成します.

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

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

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

  //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
  while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  move_base_msgs::MoveBaseGoal goal;

  //we'll send a goal to the robot to move 1 meter forward
  goal.target_pose.header.frame_id = "base_footprint";
  goal.target_pose.header.stamp = ros::Time::now();

  goal.target_pose.pose.position.x = 1.0;
  goal.target_pose.pose.orientation.w = 1.0;

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

  ac.waitForResult();

  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Hooray, the base moved 1 meter forward");
  else
    ROS_INFO("The base failed to move forward 1 meter for some reason");

  return 0;
}

3. CMakeLists.txtの生成・編集

いつものように,catkinビルドのためにCMakeLists.txtを変更します.

cmake_minimum_required(VERSION 2.8.3)
project(simple_goal_generator)

find_package(catkin REQUIRED COMPONENTS
  actionlib
  move_base_msgs
  roscpp
)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES simple_goal_generator
  CATKIN_DEPENDS actionlib move_base_msgs roscpp
  DEPENDS system_lib
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(simple_goal_generator_node src/simple_goal_generator.cpp)

target_link_libraries(simple_goal_generator_node
  ${catkin_LIBRARIES}
)

4. ノードの起動

roscore
rosrun simple_goal_generator simple_goal_generator_node

move_baseノードが起動している状態で上記ノードを起動すると,goal座標をmove_baseに送ってくれます.
送信後は,下記のようなログが出力されます.

[ INFO] [1486816759.533561032]: Sending goal