ROS Coding.3.Robot Model အတြက္　Moving Forward Program တစ္ခုေရးသားျခင္း

အရင္　post မွာကေတာ့　robot model ကို　keyboard ကေနတဆင့္　ခိုင္းေစခဲ့ပါတယ္။　ကြ်န္ေတာ္က　theory နဲ႕ေရာခ်ျပီး　keyboard ကေန　တဆင့္ခိုင္းေစနိုင္ဖို႕　ၾကားခံ　node တစ္ခုေရးတာကို　ရွင္းျပခဲ့ပါတယ္။　ros မွာ　robot ေတြရဲ႕　ေရြ႕လ်ားမွဳကို　Twist ဆိုတဲ့ message ကေန　တဆင့္ခိုင္းေစပါတယ္။　Twist မွာ　ဘာေတြပါလဲဆိုတာကို　ေအာက္ပါ　command ကေနတဆင့္ၾကည့္ပါမယ္။

rosmsg show Twist

Twist msg မွာပါတဲ့　data type ေတြကို　ေအာက္ပါအတိုင္းေဖာ္ျပေပးပါတယ္။

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

အေရွ႕　tutorial မွာ　ေရးခဲ့တဲ့　node က　ဒီ　twist msg data ကို　subscribe လုပ္ေနတာျဖစ္ပါတယ္။　subscribe လုပ္ျပီး　base_link နဲ႕　odom frame နွစ္ခုကို　အျမဲ　translation လုပ္ျပီးျပေပးေနတာျဖစ္ပါတယ္။　ဒီမွာေတာ့　twist msg ကို　program ကေန　ထုတ္ေပးျပီး　robot အတြက္　forward program တစ္ခုကိုေရးၾကည့္ရေအာင္။ myrobot1/src ထဲမွာ　forward.cpp ဆိုတဲ့　ဖိုင္တစ္ခုကိုဖန္တီးျပီး　ေအာက္ပါ　code ေတြကို　ကူးထည့္ပါမယ္။

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <sstream>

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

    ros::NodeHandle n;
    ros::Publisher pub=n.advertise<geometry_msgs::Twist>("cmd_vel",10);

    int rate=10;
    float linear_velocity=0.2;
    float goal_distance=1.0;
    float time_duration;
    int ticks;

    time_duration=goal_distance/linear_velocity;
    ticks=rate*time_duration;

    geometry_msgs::Twist move_cmd;
    move_cmd.linear.x=linear_velocity;

    ros::Rate r(rate);

    for(int i=0;i<ticks;i++)
    {
        pub.publish(move_cmd);
        r.sleep();
    }

    move_cmd.linear.x=0.0;
    pub.publish(move_cmd);
    ros::Duration(1).sleep();
}

ros::init(argc,argv,"forward");
ros::NodeHandle n; 
ros::Publisher pub=n.advertise<geometry_msgs::Twist>("cmd_vel",10);

node နာမည္ကိုေတာ့　forward ဆုိတဲ့　name နဲ႕　initialize လုပ္လိုက္ပါတယ္။　topic ေတြကို　publish လုပ္မွာျဖစ္တဲ့အတြက္　pub ဆုိတဲ့　publisher တစ္ခုကိုလည္း　ဖန္တီးထားပါတယ္။

float linear_velocity=0.2; 
float goal_distance=1.0; 
float time_duration;

ဒီမွာေတာ့　linear_velocity တန္ဖိုးနဲ႕　goal_distance တန္ဖိုးကို　သတ္မွတ္ေပးထားပါတယ္။

time_duration=goal_distance/linear_velocity;
ticks=rate*time_duration;

time_duration ကိုေတာ့　v=m/s ဆိုတဲ့ပံုေသနည္းကို　တြက္ျပီးယူထားတယ္။　goal_distance 1 m ကို　velocity 0.2 m/s နဲ႕သြားရင္　ဘယ္ေလာက္ၾကာၾကာသြားရမလဲေပါ့ဗ်ာ။　ticks တန္ဖိုးကို　တြက္ယူထားတာက　publisher သည္　frequency တစ္ခါခုန္တိုင္း　တစ္ၾကိမ္　publish လုပ္ပါတယ္။　ဒီမွာ　ကြ်န္ေတာ္တို႕က　frequecy ကို　၁၀　လို႕ေပးထားေတာ့　1 second ကို　ဆယ္ၾကိမ္　publish လုပ္ပါလိမ့္မယ္။　တြက္လို႕ရတဲ့တန္ဖိုးက　5 second ဆုိေတာ့　အၾကိမ္　၅၀　မျပည့္မခ်င္း　publish လုပ္ေပးေနပါလိမ့္မယ္။　ေရးျပီးတာနဲ႕　executable ဖိုင္ရဖို႕အတြက္ကို　CMakeLists.txt ထဲမွာ　ေအာက္ပါစာသား　၂　ေၾကာင္းကို　သြားျဖည့္ျပီး　compile လုပ္လိုက္ပါမယ္။

add_executable(forward src/forward.cpp)
target_link_libraries(forward ${catkin_LIBRARIES})

python နဲ႕လည္း　ေရးထားေပးပါတယ္။

#!/usr/bin/env python

import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist

class Forward():
    def __init__(self):
        rospy.init_node('forward',anonymous=True)
        self.pub=rospy.Publisher('cmd_vel',Twist,queue_size=10)
        rate=10
        r=rospy.Rate(rate)

        linear_velocity=0.2
        goal_distance=1
        time_duration=goal_distance/linear_velocity

        move_cmd=Twist()
        move_cmd.linear.x=linear_velocity
        ticks=int(rate*time_duration)
        for t in range(ticks):
            self.pub.publish(move_cmd)
            r.sleep()

        move_cmd=Twist()
        self.pub.publish(move_cmd)
        rospy.sleep(1)

if __name__=='__main__':
    try:
        Forward()
    except rospy.ROSInterruptException:
        rospy.loginfo("shutdown")

run ၾကည့္လိုက္ရင္ေတာ့　ေအာက္ပါပံုအတိုင္း　ျမင္ရပါလိမ့္မယ္။