ROS.3.2.Moving Square Using Odometry

အရင္　program မွာတုန္းကေတာ့　robot ကို　သြားရမယ့္　distance ေပးျပီး ဘာ　feedback မွ　ျပန္မယူပဲ　သြားခိုင္းလိုက္တယ္။　robot ရဲ႕　လက္ရွိတည္ေနရာနဲ႕　သြားခိုင္းထားတဲ့　distance ကိုက္ညီရဲ႕လားဆိုတာကို　ျပန္လည္မစစ္ေဆးခဲ့ဘူး။။　ဒီ　program မွာေတာ့　robot ရဲ႕　လက္ရွိေရာက္ေနတဲ့　တည္ေနရာနဲ႕　target ကိုေရာက္မေရာက္ဆိုတာကို　အျမဲစစ္ေဆးထားတယ္။

#include <ros/ros.h>
#include <ros/time.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TransformStamped.h>
#include <std_msgs/String.h>
#include <math.h>
#include <stdio.h>

using namespace std;


float normalize_angle(float angle);

int main(int argc,char** argv)
{
    ros::init(argc,argv,"odom_out_and_back");
    ros::NodeHandle n;
    ros::Publisher cmd_vel=n.advertise<geometry_msgs::Twist>("cmd_vel",10);

    int rate=10;
    float linear_speed=0.2;
    float goal_distance=1.0;
    float angular_speed=0.5;
    float angular_tolarance=0.0175; //rad for 1 degree
    float goal_angle=1.5708;  //90 degree
    ros::Rate r(rate);

    tf::TransformListener tf_listener;
    tf::StampedTransform transform;

    //Give tf some time to fill its buffer
    ros::Duration(2).sleep();

    string odom_frame="odom";
    string base_frame;

    try
    {
        tf_listener.waitForTransform(odom_frame,"base_link",ros::Time(),ros::Duration(1.0));
        base_frame="base_link";
    }
    catch(tf::LookupException ex)
    {
        ROS_INFO("cannot find transform between /odom and /base_link or /base_footprint");
    }

    for(int i=0;i<4;i++)
    {
        geometry_msgs::Twist move_cmd;
        move_cmd.linear.x=linear_speed;
        move_cmd.angular.z=0.0;
        try
        {
            tf_listener.lookupTransform(odom_frame,base_frame,ros::Time(0),transform);
        }
        catch(tf::TransformException ex)
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
        }

        float x_start=transform.getOrigin().x();
        float y_start=transform.getOrigin().y();
        float distance=0;
        while(distance < goal_distance)
        {
            cmd_vel.publish(move_cmd);
            r.sleep();
            try
            {
                tf_listener.lookupTransform(odom_frame,base_frame,ros::Time(0),transform);
            }
            catch(tf::TransformException ex)
            {
                ROS_ERROR("%s",ex.what());
                ros::Duration(1.0).sleep();
            }
            distance=sqrt(pow(transform.getOrigin().x()-x_start,2)+
                          pow(transform.getOrigin().y()-y_start,2));
        }

        cmd_vel.publish(geometry_msgs::Twist());
        ros::Duration(1.0).sleep();

        move_cmd.linear.x=0.0;
        move_cmd.angular.z=angular_speed;
        tf::Matrix3x3 m(transform.getRotation());
        double roll,pitch,yaw;
        m.getRPY(roll,pitch,yaw);
        double last_angle=yaw;
        double turn_angle=0;
        while(abs(turn_angle+angular_tolarance) < abs(goal_angle))
        {
            cmd_vel.publish(move_cmd);
            r.sleep();
            try
            {
                tf_listener.lookupTransform(odom_frame,base_frame,ros::Time(0),transform);
            }
            catch(tf::TransformException ex)
            {
                ROS_ERROR("%s",ex.what());
                ros::Duration(1.0).sleep();
            }
            tf::Matrix3x3 m(transform.getRotation());
            m.getRPY(roll,pitch,yaw);
            double rotation=yaw;
            double delta_angle=normalize_angle(rotation - last_angle);
            turn_angle += delta_angle;
            last_angle = rotation;
        }

        cmd_vel.publish(geometry_msgs::Twist());
        ros::Duration(1.0).sleep();
    }

    cmd_vel.publish(geometry_msgs::Twist());
    ros::Duration(1.0).sleep();
}

float normalize_angle(float angle)
{
    float res=angle;
    while(res > 3.1416)
    {
        res -= 2.0 * 3.1416;
    }

    while(res < -3.1416)
    {
        res += 2.0 * 3.1416;
    }
    return res;
}

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

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist,Point,Quaternion
import tf
from transform_utils import quat_to_angle,normalize_angle
from math import radians,copysign,sqrt,pow,pi

class OutAndBack():
    def __init__(self):
        rospy.init_node('odom_out_and_back',anonymous=True)
        rospy.on_shutdown(self.shutdown)
        self.cmd_vel=rospy.Publisher('cmd_vel',Twist,queue_size=10)
        rate=10
        r=rospy.Rate(rate)

        linear_speed=0.15
        goal_distance=1.0
        angular_speed=0.5
        angular_tolarance=radians(1.0)
        goal_angle=pi/2

        self.tf_listener=tf.TransformListener()

        #Give tf some time to fill its buffer
        rospy.sleep(2)
        #set the odom frame
        self.odom_frame='/odom'

        try:
            self.tf_listener.waitForTransform(self.odom_frame,'base_footprint',rospy.Time(),rospy.Duration(1.0))
            self.base_frame='/base_footprint'
        except (tf.Exception,tf.ConnectivityException,tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame,'base_link',rospy.Time(),rospy.Duration(1.0))
                self.base_frame='/base_link'
            except (tf.Exception,tf.ConnectivityException,tf.LookupException):
                rospy.loginfo("cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")

        position=Point()
        for i in range(4):
            move_cmd=Twist()
            move_cmd.linear.x=linear_speed
            (position,rotation)=self.get_odom()
            x_start=position.x
            y_start=position.y
            distance=0
            while distance < goal_distance and not rospy.is_shutdown():
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                (position,rotation)=self.get_odom()
                distance=sqrt(pow((position.x - x_start),2)+pow((position.y - y_start),2))

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

            move_cmd.angular.z=angular_speed
            last_angle=rotation
            turn_angle=0
            while abs(turn_angle+angular_tolarance) < abs(goal_angle) and not rospy.is_shutdown():
                    self.cmd_vel.publish(move_cmd)
                    r.sleep()
                    (position,rotation)=self.get_odom()
                    delta_angle=normalize_angle(rotation - last_angle)

                    turn_angle += delta_angle
                    last_angle = rotation

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

        self.cmd_vel.publish(Twist())
        rospy.sleep(1)

    def get_odom(self):
        try:
            (trans,rot)=self.tf_listener.lookupTransform(self.odom_frame,self.base_frame,rospy.Time(0))
        except (tf.Exception,tf.ConnectivityException,tf.LookupException):
            rospy.loginfo("TF Exception")
            return
        return (Point(*trans),quat_to_angle(Quaternion(*rot)))

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)

if __name__=='__main__':
    try:
        OutAndBack()
    except rospy.ROSInterruptException:
        rospy.loginfo("out_and_back node terminated.")