SHARE
TWEET

Untitled

a guest Mar 25th, 2019 66 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <ros/ros.h>
  2. #include <geometry_msgs/Twist.h>
  3. #include <turtlesim/Pose.h>
  4.  
  5. #include <iostream>
  6. #include <queue>
  7. #include <iomanip>
  8. using namespace std;
  9.  
  10. float x, y, theta, v, vt;
  11. float prevx, prevy;
  12. int state = 0;
  13. float target_angle;
  14. float target_distance;
  15. ros::Publisher pub;
  16. const float PI = 3.14159265;
  17. float rate = 30;
  18.  
  19. geometry_msgs::Twist getMessage(double linear_x, double angular_z)
  20. {
  21.     geometry_msgs::Twist msg;
  22.     msg.linear.x = linear_x;
  23.     msg.angular.z = angular_z;
  24.     return msg;
  25. }
  26.  
  27. void handleStateRotate()
  28. {
  29.     if (abs(target_angle-theta) > 1.0/rate) {
  30.         pub.publish(getMessage(0, (target_angle-theta)));
  31.     } else if (abs(target_angle-theta) > 1e-3) {
  32.         pub.publish(getMessage(0, (target_angle-theta)*rate));
  33.     } else {
  34.         pub.publish(getMessage(0, 0));
  35.         state = 0;
  36.     }
  37. }
  38.  
  39. void handleStateStraightForward()
  40. {
  41.     cout << "target=" << target_distance << endl;
  42.     if (target_distance <= 1e-6) {
  43.         pub.publish(getMessage(0, 0));
  44.         state = 0;
  45.     } else {
  46.         if (target_distance > 1.0 / rate)
  47.             pub.publish(getMessage(1, 0));
  48.         else
  49.             pub.publish(getMessage(target_distance*rate, 0));
  50.     }
  51. }
  52.  
  53. void poseCallback(const turtlesim::Pose::ConstPtr& msg)
  54. {
  55.     static bool firstCall = true;
  56.     // cout << "firstCall=" << boolalpha << firstCall << endl;
  57.     prevx = x, prevy = y;
  58.     // cout << "x=" << msg->x << " y=" << msg->y
  59.     //      << " theta=" << msg->theta << " v=" << msg->linear_velocity
  60.     //      << " vtheta=" << msg->angular_velocity << endl;
  61.     x = msg->x, y = msg->y, theta = msg->theta,
  62.     v = msg->linear_velocity, vt = msg->angular_velocity;
  63.  
  64.     float dist = sqrt((x-prevx)*(x-prevx)+(y-prevy)*(y-prevy));
  65.     if (!firstCall) target_distance -= dist;
  66.     firstCall = false;
  67. }
  68.  
  69. void rotate(float angle)
  70. {
  71.     state = 1;
  72.     target_angle = angle;
  73. }
  74.  
  75. void straight_forward(float distance)
  76. {
  77.     state = 2;
  78.     target_distance = distance;
  79. }
  80.  
  81. struct Action
  82. {
  83.     int type;
  84.     float target_angle;
  85.     float target_distance;
  86. };
  87.  
  88.  
  89. int main(int argc, char** argv)
  90. {
  91.     ros::init(argc, argv, "myturtle_control");
  92.     ros::NodeHandle h;
  93.     pub = h.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
  94.     ros::Subscriber sub =
  95.         h.subscribe("/turtle1/pose", 1000, poseCallback);
  96.     ros::Rate loopRate(rate);
  97.  
  98.     float tx = 8, ty = 8;
  99.  
  100.     queue<Action> q;
  101.     q.push({2, 0, 2});
  102.     q.push({1, PI/2, 0});
  103.     q.push({2, 0, 2});
  104.     q.push({1, PI, 0});
  105.     q.push({2, 0, 2});
  106.     q.push({1, 3*PI/2, 0});
  107.     q.push({2, 0, 2});
  108.     q.push({1, 2*PI, 0});
  109.  
  110.  
  111.     q.push({2, 0, 2});
  112.     q.push({1, PI/2, 0});
  113.     q.push({2, 0, 2});
  114.     q.push({1, PI, 0});
  115.     q.push({2, 0, 2});
  116.     q.push({1, 3*PI/2, 0});
  117.     q.push({2, 0, 2});
  118.     q.push({1, 2*PI, 0});
  119.  
  120.     bool in_action = false;
  121.     while (ros::ok()) {
  122.         //pub.publish(getMessage(linear_x, 5.0));
  123.         //linear_x += 1.0;
  124.         if (state == 0 && !in_action) {
  125.             if (q.size() > 0) {
  126.                 Action a = q.front();
  127.                 q.pop();
  128.                 if (a.type == 1) rotate(a.target_angle);
  129.                 else if (a.type == 2) straight_forward(a.target_distance);
  130.                 in_action = true;
  131.                 cout << "state=" << state
  132.                      << " action=" << a.type << " " << a.target_distance << " " << a.target_angle << endl;
  133.             }
  134.         }
  135.         else if (state == 0 && in_action) {
  136.             in_action = false;
  137.         }
  138.         else if (state == 1) handleStateRotate();
  139.         else if (state == 2) handleStateStraightForward();
  140.        loopRate.sleep();
  141.         ros::spinOnce();
  142.     }
  143.  
  144.     return 0;
  145. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top