簡單的程式碼讓turtlebot動起來II（使用kobuki底座）走三角形
阿新 • • 發佈：2019-02-18
接著上一篇turtlebot的直行和旋轉。
ROS中，節點之間通過訊息溝通，主題相當於是對訊息進行路由和管理的匯流排。節點向主題釋出訊息即表示一個節點在傳送資料；節點訂閱某個主題即可以接受其他節點的訊息。在上一篇的直行和旋轉當中，我們只寫一個publisher，僅僅有話題的釋出並沒有話題的訂閱。於是就有了嘗試寫一個subsrciber的想法，實現turtlebot稍微複雜一點的運動。
想要讓turtlebot走一個三角形，那麼同上一篇，首先我們需要向底座釋出速度的資訊，publisher有了。那麼subscriber是什麼呢？三角形有自己的邊長，turtlebot要知道自己走了多遠，就需要知道自己的里程計資訊（轉載的一篇里程計相關的文章有介紹），即訂閱/odom主題。寫一個訂閱者的步驟：（1）初始化ROS系統（2）訂閱話題（3）Spin，等待訊息的到來（4）當一個訊息到達時，相應的Callback()函式被呼叫。
有了思路就可以實現了，附上程式碼：
// 標頭檔案velcontrol.h #ifndef VELCONTROL_H #define VELCONTROL_H #include <ros/ros.h> #include <geometry_msgs/Twist.h> #include <geometry_msgs/TwistStamped.h> #include <nav_msgs/Odometry.h> #include "tf/LinearMath/Matrix3x3.h" #include "geometry_msgs/Quaternion.h" #define CV_PI 3.1415926535897932384626433832795 #include <iostream> #include <stdlib.h> using namespace std ; typedef struct roPos { double x; double y; double theta; void init(double x1, double x2, double x3) { x = x1; y = x2; theta = x3; } } roPos; class velControl { public: velControl(); roPos start_pos_; roPos curr_pos_; protected: void odomCallback(const nav_msgs::Odometry::ConstPtr& msg); private: double rect_width_; double rect_height_; double offset_xy_; double offset_theta_; double vel_line_; double vel_angle_; int state_; bool Is_Fininsh_; ros::NodeHandle handle_; ros::Publisher vel_pub_ ; ros::Subscriber sub_ ; geometry_msgs::Twist controlVel_; private: double angleWrap(double angle); bool init(); bool velocityControl(); int vel_pub_speed_; }; #endif // VELCONTROL_H
//執行檔案 velcontrol.cpp #include "velcontrol.h" velControl::velControl() { init(); vel_pub_ = handle_.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity",1); sub_ = handle_.subscribe("/odom", 1, &velControl::odomCallback,this); //訂閱"/odom"話題，當一個新的訊息到達時，ROS將會呼叫velControl::odomCallback()函式。第二個引數是對列的長度，如果我們處理訊息的速度不夠快，會將收到的訊息緩衝下來，滿了的話，後面的到達的訊息將會覆蓋前面的訊息。 } bool velControl::init() { start_pos_.init(0.0, 0.0, 0.0); curr_pos_.init(0.0, 0.0, 0.0); rect_width_ = 0.5; vel_line_ = 0.2; vel_angle_ = 0.2; vel_pub_speed_ = 5; state_ = 0; Is_Fininsh_ = true; } void velControl::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) { static int count = 0; geometry_msgs::Quaternion orientation = msg->pose.pose.orientation; tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)); double yaw, pitch, roll; mat.getEulerYPR(yaw, pitch, roll); //Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR. //Yaw around Z axis; Pitch around Y axis; Roll around X axis curr_pos_.x = msg->pose.pose.position.x; curr_pos_.y = msg->pose.pose.position.y; curr_pos_.theta = yaw; if(Is_Fininsh_) { start_pos_.x = curr_pos_.x; start_pos_.y = curr_pos_.y; start_pos_.theta = curr_pos_.theta; } count++; if( count %= vel_pub_speed_ ) { velocityControl(); } } bool velControl::velocityControl() { geometry_msgs::Twist controlVel_; switch (state_) { case 0: //走一個邊長，以下同理 if ( abs(curr_pos_.x - start_pos_.x) < rect_width_ ) { cout<<"0 curr_pos_x "<<abs(curr_pos_.x - start_pos_.x) <<endl; controlVel_.angular.z = 0.0; controlVel_.linear.x = vel_line_; controlVel_.linear.y = 0; controlVel_.linear.z = 0; Is_Fininsh_ = false; } else { cout<<"0 curr_pos_x "<<abs(curr_pos_.x - start_pos_.x) <<endl; controlVel_.angular.z = 0.0; controlVel_.linear.x = 0.0; controlVel_.linear.y = 0; controlVel_.linear.z = 0; state_ = 1; Is_Fininsh_ = true; } break; case 1: //旋轉120度，以下同理 if ( abs(angleWrap(curr_pos_.theta - start_pos_.theta)) < CV_PI/3*2 ) { cout<<"1 curr_pos_theta "<<curr_pos_.theta - start_pos_.theta<<endl; controlVel_.angular.z = vel_angle_; controlVel_.linear.x = 0; controlVel_.linear.y = 0; controlVel_.linear.z = 0; Is_Fininsh_ = false; } else { cout<<"1 curr_pos_theta "<<curr_pos_.theta - start_pos_.theta<<endl; controlVel_.angular.z = 0.0; controlVel_.linear.x = 0.0; controlVel_.linear.y = 0; controlVel_.linear.z = 0; state_ = 2; Is_Fininsh_ = true; } break; case 2: if ( abs(curr_pos_.y - start_pos_.y ) < rect_width_) { cout<<"2 curr_pos_y "<<abs(curr_pos_.y - start_pos_.y ) <<endl; controlVel_.angular.z = 0.0; controlVel_.linear.x = vel_line_; controlVel_.linear.y = 0; controlVel_.linear.z = 0; Is_Fininsh_ = false; } else { cout<<"2 curr_pos_y "<<abs(curr_pos_.y - start_pos_.y ) <<endl; controlVel_.angular.z = 0.0; controlVel_.linear.x = 0.0; controlVel_.linear.y = 0; controlVel_.linear.z = 0; state_ = 3; Is_Fininsh_ = true; } break; case 3: if ( abs(angleWrap(curr_pos_.theta - start_pos_.theta)) < CV_PI/3*2 ) { cout<<"3 curr_pos_theta "<< abs(angleWrap(curr_pos_.theta - start_pos_.theta)) <<endl; controlVel_.angular.z = vel_angle_; controlVel_.linear.x = 0; controlVel_.linear.y = 0; controlVel_.linear.z = 0; Is_Fininsh_ = false; } else { cout<<"3 curr_pos_theta "<<abs(angleWrap(curr_pos_.theta - start_pos_.theta)) <<endl; controlVel_.angular.z = 0.0; controlVel_.linear.x = 0.0; controlVel_.linear.y = 0; controlVel_.linear.z = 0; state_ = 4; Is_Fininsh_ = true; } break; case 4: if ( abs(curr_pos_.x - start_pos_.x) < rect_width_ ) { cout<<"4 curr_pos_x "<<abs(curr_pos_.x - start_pos_.x) <<endl; controlVel_.angular.z = 0.0; controlVel_.linear.x = vel_line_; controlVel_.linear.y = 0; controlVel_.linear.z = 0; Is_Fininsh_ = false; } else { cout<<"4 curr_pos_x "<<abs(curr_pos_.x - start_pos_.x) <<endl; controlVel_.angular.z = 0.0; controlVel_.linear.x = 0.0; controlVel_.linear.y = 0; controlVel_.linear.z = 0; state_ = 5; Is_Fininsh_ = true; } break; case 5: if ( abs(angleWrap(curr_pos_.theta - start_pos_.theta)) < CV_PI/3*2 ) { cout<<"5 curr_pos_theta "<<abs(angleWrap(curr_pos_.theta - start_pos_.theta)) <<endl; controlVel_.angular.z = vel_angle_; controlVel_.linear.x = 0; controlVel_.linear.y = 0; controlVel_.linear.z = 0; Is_Fininsh_ = false; } else { cout<<"5 curr_pos_theta "<<abs(angleWrap(curr_pos_.theta - start_pos_.theta)) <<endl; controlVel_.angular.z = 0.0; controlVel_.linear.x = 0.0; controlVel_.linear.y = 0; controlVel_.linear.z = 0; state_ = 6; Is_Fininsh_ = true; } break; default: controlVel_.angular.z = 0.0; controlVel_.linear.x = 0.0; controlVel_.linear.y = 0; controlVel_.linear.z = 0; Is_Fininsh_ = true; break; } vel_pub_.publish(controlVel_); } double velControl::angleWrap(double angle) { //把角度規劃到-pi至pi之間 if (angle>=CV_PI) while (angle >= CV_PI) { angle = angle-2*CV_PI;} else if (angle < -1.0*CV_PI) while (angle < -1.0*CV_PI) { angle = angle+2*CV_PI;} return angle; } int main(int argc,char** argv) { ros::init(argc,argv,"odompub"); velControl odom_vel_; ros::spin(); //ros::spin()進入了一個迴圈，可以儘快的呼叫訊息的回撥函式。不要擔心，如果它沒有什麼事情可做時，它也不會浪費太多的CPU。當ros::ok()返回false時，ros::spin()將會退出。這就意味著，當ros::shutdown()被呼叫，或按下CTRL+C等情況，都可以退出。 return 0; }
ros::spin()和ros::spinonce()的區別在於：前者呼叫後不在返回，後者呼叫後繼續執行後面的程式。在使用ros::spin()的情況下，一般來說在初始化時已經設定好所有訊息的回撥，並且不需要其他背景程式執行。這樣以來，每次訊息到達時會執行使用者的回撥函式進行操作，相當於程式是訊息事件驅動的；而在使用ros::spinOnce()的情況下，一般來說僅僅使用回撥不足以完成任務，還需要其他輔助程式的執行：比如定時任務、資料處理、使用者介面等。
修改相應的CMakeList.txt，執行節點即可。但是觀察到turtlebot執行的軌跡並不是完美的正三角形，走起來的誤差還是存在的，誤差應該是和里程計有關。
最後感謝騰哥的指導！