#ifndef TURTLE__BOT__HH__
#define TURTLE__BOT__HH__
#include "robot_base.h"
#include <turtlesim/Pose.h>

class TurtleRobot: public RobotBase
{
public:
    TurtleRobot()
    {
        
        pose_sub_ = nh_.subscribe<turtlesim::Pose>("/turtle1/pose", 1,boost::bind(&TurtleRobot::pose_callback,_1,this));
    }
    ~TurtleRobot(){}
protected:
    static void pose_callback(const turtlesim::Pose::ConstPtr& msg,TurtleRobot* robot)
    {
        if(robot==0) return ;
        ros::Time current_time;
        current_time = ros::Time::now();
        double theta=msg->theta;
        //std::cout<<" turtle angular:"<<theta*180.0/M_PI<<std::endl;
        robot->base_link_pose_.setOrigin(tf::Vector3(msg->x,msg->y,0.0));
        tf::Quaternion q;
        q.setRPY(0,0,theta);
        robot->base_link_pose_.setRotation(q);
        
        robot->current_linear_speed_=msg->linear_velocity;
        robot->current_angular_speed_=msg->angular_velocity;
    }
    bool post_tf()
    {
        
        static tf::TransformBroadcaster br;
        br.sendTransform(tf::StampedTransform(base_link_pose_, ros::Time::now(), "map", "base_link"));
        tf::Transform tf;
        tf.setOrigin(tf::Vector3(0,0,0));
        tf.setRotation(tf::Quaternion(0,0,0,1));
        br.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "base_link", "odom"));
       
    }
    bool get_odom()
    {
        return true;
    }
    bool down_speed()
    {
        static ros::Publisher cmd_vel_publisher_=nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1, true);

        geometry_msgs::Twist twist;
        twist.linear.x=cmd_vel_linear_;
        twist.linear.y=0;
        twist.linear.z=0;
        twist.angular.x=0;
        twist.angular.y=0;
        twist.angular.z=cmd_vel_angular_;
    
        cmd_vel_publisher_.publish(twist);
    }

protected:
    ros::Subscriber 	pose_sub_;
    
};

#endif // !TURTLE__BOT__HH__