ROS hydroでTurtlebot2を（シュミレータ上で）動かしてみる２（オドメトリ情報からの動作）

前置きは無しにしてぱぱっと書きます
 ROS hydroのgazeboを使ってTurtlebot2を動かします。

方法は簡単で前回記事にあったTurtlebot2のトピックリストから使える情報を取得して速度情報を送るという感じです。
今回はオドメトリの情報を入力、出力を速度情報という形で動作させます。

流れは下のようになります 。
1.gazeboのワールド上にTurtlebotのみを表示
 $ roslaunch turtlebot_gazebo turtlebot_empty_world.launch

2.動作プログラムを走らせる
動作プログラムは下のような形で作りました

#include<iostream>
#include<math.h>
#include<ros/ros.h>
#include<nav_msgs/Odometry.h>
#include<geometry_msgs/Twist.h>

using namespace std;

namespace{
    double poseX;
    double poseY;
    double poseT;
}

void odomCallback(const nav_msgs::Odometry::ConstPtr &odom)
{
    poseX = odom->pose.pose.position.x;
    poseY = odom->pose.pose.position.y;
    poseT = odom->pose.pose.orientation.w;

    cout<<"x = "<<poseX<<"[m]"<<"\t";
    cout<<"y = "<<poseY<<"[m]"<<"\t";
    cout<<"theta = "<<poseT*180/M_PI<<"[deg]"<<endl;
}

int main(int argc,char **argv)
{
    ros::init(argc,argv,"kobuki_navi");
    ros::NodeHandle node;

    ros::Subscriber odom_sub = node.subscribe("odom",1,odomCallback);
    ros::Publisher vel_pub = node.advertise<geometry_msgs::Twist>("cmd_vel",1);

    geometry_msgs::Twist cmd_vel;

    while(ros::ok())
    {
        if(poseX < 5.0)
        {
            cmd_vel.linear.x = 0.2;
            cmd_vel.linear.y = 0.0;
            cmd_vel.angular.z = 0.0;
        }
        else
        {
            cmd_vel.linear.x = 0.0;
            cmd_vel.linear.y = 0.0;
            cmd_vel.angular.z = 0.0;
        }
        vel_pub.publish(cmd_vel);
        ros::spinOnce();
    }
    return 0;
}

このプログラムはパブリッシュするトピックを”cmd_vel"にしているので起動時に以下のようにリマップしてください

$ rosrun パッケージ名 実行ファイル名 cmd_vel:=/mobile_base/commands/velocity

間違い訂正に気づいたら追記行います。

雑記
最初に参考にしたのがキーボード入力を行なってkobukiを動かすというものを参考にしていたのでモーター電源ONにしないと走らないんじゃ！！？と思ってモータ電源入れるプログラムも作ったけど実際後で動かしてみると要らなかったみたい。無くても普通に動き出した。

実機の場合はいるのかなぁ？
次は搭載しているセンサ使っての簡単なナビゲーションかな