#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose2D.h>
#include <tf/transform_listener.h>
#include <stdio.h>
class test
{
private:
    ros::NodeHandle nh_;
    ros::Subscriber sub_;
    tf::TransformBroadcaster br_;
    bool data_ready;

public:
    test(ros::NodeHandle& nh)
    {
        nh_=nh;
        sub_ = nh_.subscribe("/user_set", 10, &test::call_back,this);
        data_ready = false;
    }
    //注意这里需要加const
    void call_back(const geometry_msgs::Pose2DPtr& msgs)
    {
        ROS_INFO("recive");
        tf::Transform dest_transform;
        dest_transform.setOrigin(tf::Vector3(msgs->x,msgs->y,0));
        tf::Quaternion q;
        q.setRPY(0, 0, msgs->theta);
        dest_transform.setRotation(q);
        br_.sendTransform(tf::StampedTransform(dest_transform,ros::Time::now(),"world","user_set_frame"));
        data_ready = true;
    }

    bool is_data_ready()
    {
        if(data_ready)
            return true;
        else
            return false;
    }
};
int main(int argc, char** argv)
{
  ros::init(argc, argv, "tf_broadcaster");
  ros::NodeHandle node;
  test Otest(node);
  tf::TransformListener listener;
  tf::StampedTransform transform;

  while(ros::ok())
  {
    if(!Otest.is_data_ready())
    {
        ros::spinOnce();

        continue;
    }
  　ROS_INFO("lookup_transfoem;");
    try
    {
      listener.waitForTransform("world","user_set_frame",ros::Time::now(), ros::Duration(1.0));
      listener.lookupTransform("world","user_set_frame",ros::Time(0),transform);
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s",ex.what());
      break;
    }
    tf::Vector3 vectortf = transform.getOrigin();
    ROS_INFO("transform.x:%f,transform.y:%f,transform.z:%f",vectortf.x(),vectortf.y(),vectortf.z());
    ros::spinOnce();
  }
  return 0;
}

#include <iostream>

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

class RobotDriver
{
private:
  //! The node handle we‘ll be using
  ros::NodeHandle nh_;
  //! We will be publishing to the "cmd_vel" topic to issue commands
  ros::Publisher cmd_vel_pub_;
  //! we will be suscribing the topic "pos_dest"
  ros::Subscriber sub = n.subscribe("pos_dest", 10, pos_dest_callback);
  //! pos_dest_callback
  tf::Transform dest_transform;
  void pos_dest_callback(geometry_msgs::Pose2DPtr& msgs)
  {
      static tf::TransformBroadcaster br;
      dest_transform.setOrigin(tf::Vector3(msgs->x,msgs->y,0));
      tf::Quaternion q;
      q.setRPY(0, 0, msgs->theta);
      dest_transform.setRotation(q);
      br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "pos_dest"));

  }
  //! We will be listening to TF transforms as well
  tf::TransformListener listener_;
  //! cmd
  geometry_msgs::Twist base_cmd_straight;
  geometry_msgs::Twist base_cmd_turn;
  geometry_msgs::Twist stop_cmd;
  tf::Transform current_transform;


public:
  //! ROS node initialization
  RobotDriver(ros::NodeHandle &nh)
  {
    nh_ = nh;
    //set up the publisher for the cmd_vel topic
    cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    //cmd
    //the command will be to go forward at 0.25 m/s
    base_cmd_straight.linear.y = base_cmd_straight.angular.z = 0;
    base_cmd.base_cmd_straight.x = 0.1;

    base_cmd_turn.linear.x = base_cmd_turn.linear.y = 0.0;
    base_cmd_turn.angular.z = 0.75;

    stop_cmd.linear.y = stop_cmd.angular.z = 0;
    stop_cmd.linear.x = 0;
  }



  bool turnOdom(bool clockwise, double radians)
    {
      while(radians < 0) radians += 2*M_PI;
      while(radians > 2*M_PI) radians -= 2*M_PI;

      //wait for the listener to get the first message
      listener_.waitForTransform("base_link", "odom",
                                 ros::Time(0), ros::Duration(1.0));

      //we will record transforms here
      tf::StampedTransform start_transform;
      tf::StampedTransform current_transform;

      //record the starting transform from the odometry to the base frame
      listener_.lookupTransform("base_link", "odom",
                                ros::Time(0), start_transform);

      //we will be sending commands of type "twist"
      geometry_msgs::Twist base_cmd;
      //the command will be to turn at 0.75 rad/s
      base_cmd.linear.x = base_cmd.linear.y = 0.0;
      base_cmd.angular.z = 0.75;

      geometry_msgs::Twist stop_cmd;
      stop_cmd.linear.y = stop_cmd.angular.z = 0;
      stop_cmd.linear.x = 0;


      if (clockwise) base_cmd.angular.z = -base_cmd.angular.z;

      //the axis we want to be rotating by
      tf::Vector3 desired_turn_axis(0,0,1);
      if (!clockwise) desired_turn_axis = -desired_turn_axis;

      ros::Rate rate(10.0);
      bool done = false;
      while (!done && nh_.ok())
      {
        //send the drive command
        cmd_vel_pub_.publish(base_cmd);
        rate.sleep();
        //get the current transform
        try
        {
          listener_.lookupTransform("base_link", "odom",
                                    ros::Time(0), current_transform);
        }
        catch (tf::TransformException ex)
        {
          ROS_ERROR("%s",ex.what());
          break;
        }
        tf::Transform global_transform =
          current_transform;
        tf::Vector3 actual_turn_axis =
          global_transform.getRotation().getAxis();
        ROS_INFO("actual_turn_axis.x=%f,actual_turn_axis.y=%f,actual_turn_axis.z=%f",(float)actual_turn_axis.getX(),(float)actual_turn_axis.getY(),(float)actual_turn_axis.getZ());
        double angle_turned = global_transform.getRotation().getAngle();
        if ( fabs(angle_turned) < 1.0e-2) continue;

        if ( actual_turn_axis.dot( desired_turn_axis ) < 0 )
          angle_turned = 2 * M_PI - angle_turned;

        if (angle_turned > radians) done = true;
      }
      if (done)
      {
          cmd_vel_pub_.publish(stop_cmd);

          return true;
      }
      return false;
    }



};

int main(int argc, char** argv)
{
  //init the ROS node
  ros::init(argc, argv, "robot_driver");
  ros::NodeHandle nh;

  RobotDriver driver(nh);
  driver.driveForwardOdom(0.2);
}