ROS Navigation Stack について2 ~ Odometry生成ノードの作成 ~
先ほどのmove_base生成のエントリに引き続き,Odometryを生成するノードを作成します.コードはほぼROSのチュートリアルのままです.
引用:http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom
1. Odometry生成ノード用のパッケージ作成
Odometry生成ノード用のパッケージを生成します.ここでは,simple_odom_generatorとします.
cd ~/reps/catkin_ws/src catkin_create_pkg simple_odom_generator tf nav_msgs roscd simple_odom_generator mkdir src
2. Odometryノードのソース作成
下記,ほぼチュートリアルのコードのままですが,ロボットの土台の座標系をbase_linkからbase_footprintに変えてあります.あと,簡単のために速度も位置も常に0にしてあります.
Publish Rateは10にしてます.デフォルトだと1なのですが,move_baseのデフォルトの設定だとタイムアウトしてしまうので,変えました.
#include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <nav_msgs/Odometry.h> int main(int argc, char** argv){ ros::init(argc, argv, "odometry_publisher"); ros::NodeHandle n; ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50); tf::TransformBroadcaster odom_broadcaster; double x = 0.0; double y = 0.0; double th = 0.0; double vx = 0.0; double vy = 0.0; double vth = 0.0; ros::Time current_time, last_time; current_time = ros::Time::now(); last_time = ros::Time::now(); ros::Rate r(10); while(n.ok()){ ros::spinOnce(); // check for incoming messages current_time = ros::Time::now(); //compute odometry in a typical way given the velocities of the robot //double dt = (current_time - last_time).toSec(); double dt = 0; double delta_x = (vx * cos(th) - vy * sin(th)) * dt; double delta_y = (vx * sin(th) + vy * cos(th)) * dt; double delta_th = vth * dt; x += delta_x; y += delta_y; th += delta_th; //since all odometry is 6DOF we'll need a quaternion created from yaw geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th); //first, we'll publish the transform over tf geometry_msgs::TransformStamped odom_trans; odom_trans.header.stamp = current_time; odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_footprint"; odom_trans.transform.translation.x = x; odom_trans.transform.translation.y = y; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = odom_quat; //send the transform odom_broadcaster.sendTransform(odom_trans); //next, we'll publish the odometry message over ROS nav_msgs::Odometry odom; odom.header.stamp = current_time; odom.header.frame_id = "odom"; //set the position odom.pose.pose.position.x = x; odom.pose.pose.position.y = y; odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; //set the velocity odom.child_frame_id = "base_footprint"; odom.twist.twist.linear.x = vx; odom.twist.twist.linear.y = vy; odom.twist.twist.angular.z = vth; //publish the message odom_pub.publish(odom); last_time = current_time; r.sleep(); } }
3. CMakeLists.txtの生成
次にcatkinビルドのためにCMakeLists.txtを変更します.
cmake_minimum_required(VERSION 2.8.3) project(simple_odom_generator) find_package(catkin REQUIRED COMPONENTS nav_msgs tf ) catkin_package( CATKIN_DEPENDS nav_msgs tf DEPENDS system_lib ) include_directories( ${catkin_INCLUDE_DIRS} ) add_executable(simple_odom_generator_node src/simple_odom_generator.cpp) target_link_libraries(sample_odom_generator_node ${catkin_LIBRARIES} )
4. ノードの起動
roscore rosrun simple_odom_generator simple_odom_generator_node
5. move_baseノードの起動
rosrun sample_odom_generator sample_odom_generator_node
ここまで来ると,move_baseがエラー・警告をはかなくなります.以下,実行した時のログ.
次は,このmove_baseにたいして,目的地を指定するプログラムを作ります.といっても,位置・速度は永遠に0なので,目的地に対して進むことはないプログラムですが...
core service [/rosout] found process[navigation_velocity_smoother-1]: started with pid [14221] process[kobuki_safety_controller-2]: started with pid [14222] process[move_base-3]: started with pid [14228] [ INFO] [1486805952.117027681]: Using plugin "obstacle_layer" [ INFO] [1486805952.137293082]: Subscribed to Topics: scan bump [ INFO] [1486805952.167710860]: Using plugin "inflation_layer" [ERROR] [1486805952.176414612]: You must specify at least three points for the robot footprint, reverting to previous footprint. [ INFO] [1486805952.217033572]: Using plugin "obstacle_layer" [ INFO] [1486805952.233590024]: Subscribed to Topics: scan bump [ INFO] [1486805952.261528950]: Using plugin "inflation_layer" [ERROR] [1486805952.270706401]: You must specify at least three points for the robot footprint, reverting to previous footprint. [ INFO] [1486805952.304290698]: Created local_planner dwa_local_planner/DWAPlannerROS [ INFO] [1486805952.306532365]: Sim period is set to 0.20 [ INFO] [1486805952.509251511]: Recovery behavior will clear layer obstacles [ INFO] [1486805952.528223121]: Recovery behavior will clear layer obstacles [ INFO] [1486805952.551962298]: odom received!