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