开源自主导航小车MickX4（三）底盘ROS节点
開源自主導航小車MickX4（三）底盤ROS節點
- 1、底盤ROS節點
- 1.1 ROS節點功能定義
- 1.2 接收cmd_vel話題數據
- 1.3 發布里程計數據
- 1.4 發布IMU數據
- 1.5 發布超聲波數據
- 1.6 TF坐標系
- 參考資料
底盤的ROS節點相當于是對底層硬件封裝了一個接口，讓軟件開發人員不必去關心硬件的實現。開發算法的人員只需要向指定的接口（topic）發送數據即可，底盤硬件開發人員分工合作，這也體現機器人模塊化的思想在其中。
ROS節點負責與底盤的STM32控制器通訊，從STM32中獲取底盤傳感器的狀態數據（IMU、超聲波、底盤狀態數據）；從cmd_vel 話題上獲取小車的速度指令，并將該指令以私有協議的方式下發到底盤STM32中，實現底盤的運動控制；讀取電機數據，根據電機反饋來回來的速度或者編碼器的轉角數據推算小車在世界坐標系下的坐標位置。
完整的ROS節點的代碼位于此處 [1]。
1、底盤ROS節點
1.1 ROS節點功能定義
底盤接口需要根據具體需求進行定義，通常小車底盤應當具備以下功能
1、接收其他ROS節點發布在cmd_vel話題上的速度指令，并轉化為控制命令通過串口發送給小車底盤上的STM32控制器，控制小車速度
2、接收小車底盤STM32控制板上傳的每個電機的狀態信息，根據小車模型推算出小車的位置，并發布小車里程計信息到/odom話題上
3、接收小車底盤STM32控制板上傳的IMU、超聲波傳感器數據以及程序的狀態信息。
1.2 接收cmd_vel話題數據
通常來說在ROS框架下的自主導航小車都是用的 geometry_msgs/Twist 這種消息類型來發布小車的速度命令的，而 geometry_msgs/Twist 的數據結構如下，主要包含3個方向的線速度和旋轉角速度信息，對于我們所使用的差速輪小車來說，只有X方向的線速度和Z軸方向的角速度，兩個自由度。如果是使用麥克納姆的小車底盤[2] 則這里我們可以設置X,Y方向的線速度和Z方向的角速度，此時會有3個角速度。
這部分的功能放在了 void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) 函數中進行實現，該函數從 geometry_msgs::Twist 這個結構體指針中獲取X方向的線速度和Z方向的角速度（單位：m/s），然后通過兩輪差速模型轉化為每個輪子的轉速（單位：RPM,轉每分），然后調用 send_rpm_to_chassis(v1,v2,v3,v4) 函數，按照上篇中與STM32開發板約定的協議下發每個輪子的轉速信息。
1.3 發布里程計數據
里程計的發布位于 void calculate_position_for_odometry(void) 和 void publish_odomtery(float position_x,float position_y,float oriention,float vel_linear_x,float vel_linear_y,float vel_linear_w) 兩個函數中， 其中calculate_position_for_odometry 函數負責從電機反饋的數據中計算出當前時候小車在odom坐標系下的位置、方向角和速度信息，然后調用 publish_odomtery 函數發布小車的位姿和速度信息到ROS的格式。
注意：這里我們使用的M3508電機帶有絕對位置編碼器，在使用的過程中我們發現使用絕對位置編碼器比使用速度來推算小車位置會更加的準確。
float s1=0,s2=0,s3=0,s4=0; float s1_last=0,s2_last=0,s3_last=0,s4_last=0; float position_x=0,position_y=0,position_w=0; void calculate_position_for_odometry(void) {//方法１： 計算每個輪子轉動的位移，然后利用Ｆ矩陣合成Ｘ,Y,W三個方向的位移float s1_delta=0,s2_delta=0,s3_delta=0,s4_delta=0;float v1=0,v2=0,v3=0,v4=0;float position_x_delta,position_y_delta,position_w_delta,position_r_delta;float linear_x,linear_y,linear_w;if((s1_last == 0 && s2_last == 0&& s3_last==0&&s4_last==0) || (moto_chassis[0].counter ==0)){s1 = (moto_chassis[0].round_cnt+(moto_chassis[0].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s2 = (moto_chassis[1].round_cnt+(moto_chassis[1].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s3 = (moto_chassis[2].round_cnt+(moto_chassis[2].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s4 = (moto_chassis[3].round_cnt+(moto_chassis[3].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s1_last=s1;s2_last=s2;s3_last=s3;s4_last=s4;return ;}s1_last=s1;s2_last=s2;s3_last=s3;s4_last=s4;//輪子轉動的圈數乘以 N*２*pi*rs1 = (moto_chassis[0].round_cnt+(moto_chassis[0].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s2 = (moto_chassis[1].round_cnt+(moto_chassis[1].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s3 = (moto_chassis[2].round_cnt+(moto_chassis[2].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s4 = (moto_chassis[3].round_cnt+(moto_chassis[3].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s1_delta=s1-s1_last; //每個輪子位移的增量s2_delta=s2-s2_last;s3_delta=s3-s3_last;s4_delta=s4-s4_last;// ------------------------------------------------------------------------------------------------------------------------------------------------------------------if(abs(s1_delta) < 0.001 ) s1_delta=0;if(abs(s2_delta) < 0.001 ) s2_delta=0;if(abs(s3_delta) < 0.001 ) s3_delta=0;if(abs(s4_delta) < 0.001 ) s4_delta=0;s1_delta = 0.5*s1_delta+0.5*s2_delta; s4_delta = 0.5*s3_delta+0.5*s4_delta; // if(s1_delta || s2_delta || s3_delta || s4_delta) // cout<<"s1_delta: "<<s1_delta<<" s2_delta: " <<s2_delta<<" s3_delta: " <<s3_delta<<" s4_delta: " <<s4_delta<<endl;position_w_delta =((s4_delta)- (s1_delta))/float(WHEEL_L); //w 單位為弧度if(abs(position_w_delta) < 0.0001)position_r_delta=0;elseposition_r_delta = ((s4_delta)+(s1_delta))/float(2*position_w_delta);position_x_delta=position_r_delta*sin(position_w_delta);position_y_delta = position_r_delta*(1-cos(position_w_delta));// ------------------------------------------------------------------------------------------------------------------------------------------------------------------position_x=position_x+cos(position_w)*position_x_delta-sin(position_w)*position_y_delta;position_y=position_y+sin(position_w)*position_x_delta+cos(position_w)*position_y_delta;position_w=position_w+position_w_delta;if(position_w>2*WHEEL_PI){position_w=position_w-2*WHEEL_PI; }else if(position_w<-2*WHEEL_PI){position_w=position_w+2*WHEEL_PI;}else;v1 = (moto_chassis[0].speed_rpm)/WHEEL_RATIO/60.0*WHEEL_R *WHEEL_PI*2;v2 = (moto_chassis[1].speed_rpm)/WHEEL_RATIO/60.0*WHEEL_R *WHEEL_PI*2; v3 = (moto_chassis[2].speed_rpm)/WHEEL_RATIO/60.0*WHEEL_R *WHEEL_PI*2; v4 = (moto_chassis[3].speed_rpm)/WHEEL_RATIO/60.0*WHEEL_R *WHEEL_PI*2; linear_x = 0.25*v1+ 0.25*v2+ 0.25*v3+ 0.25*v4;linear_y = 0;linear_w = ((0.5*v3+0.5*v4)-(0.5*v1+0.5*v2))/float(WHEEL_L);ROS_INFO_STREAM("px: "<<position_x<<" py: " <<position_y<<" pw: " <<position_w*57.3<<" vx: "<<linear_x<<" vy: " <<linear_y<<" vw: " <<linear_w<<endl);publish_odomtery( position_x,position_y,position_w,linear_x,linear_y,linear_w); }1.4 發布IMU數據
小車底盤上安裝的MPU9250,通過協議接口將IMU的原始數據上傳到ROS節點中，并在函數 publish_imu_mag() 中我們對底盤上傳的IMU和磁力計數據進行了發布
1.5 發布超聲波數據
超聲波模塊暫時不支持上傳，后續添加功能上傳超聲波的數據
1.6 TF坐標系
在底盤中有三個坐標系與小車底盤相關，分別是base_link,odom,laser。
base_link坐標系主要是表達小車底盤中心
laser坐標系主要是想表達激光雷達傳感器在base_link坐標系中的位姿，通常來說激光雷達都是與小車剛性連接的，因此laser與base_link之間的相對變換是固定的，因而我們可以在launch文件中靜態配置傳感器與小車底盤的關系
<launch> <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0.0 0 0.7 -3.1415926 0 0 base_link laser 50" /> </launch>上述命令中表達的是laser坐標系到base_link坐標系（表達在base_link坐標系）的偏移量X=0 y=0 z=0.7 (單位m), yaw=-π pitch=0 roll=0 單位（弧度）,關于使用四元數傳參可以參考 [3]，[4]
odom坐標系是算法根據底盤電機反饋數據而推測出小車的位置。 odom的坐標系是每次計算里程計實現更新的，在函數 void publish_odomtery(x,y,vel_x,vel_y,vel_w) 中
void publish_odomtery(float position_x,float position_y,float oriention,float vel_linear_x,float vel_linear_y,float vel_linear_w) {static tf::TransformBroadcaster odom_broadcaster; //定義tf對象geometry_msgs::TransformStamped odom_trans; //創建一個tf發布需要使用的TransformStamped類型消息geometry_msgs::Quaternion odom_quat; //四元數變量nav_msgs::Odometry odom; //定義里程計對象//里程計的偏航角需要轉換成四元數才能發布odom_quat = tf::createQuaternionMsgFromYaw(oriention);//將偏航角轉換成四元數//載入坐標（tf）變換時間戳odom_trans.header.stamp = ros::Time::now();//發布坐標變換的父子坐標系odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_link"; //tf位置數據：x,y,z,方向odom_trans.transform.translation.x = position_x;odom_trans.transform.translation.y = position_y;odom_trans.transform.translation.z = 0.0;odom_trans.transform.rotation = odom_quat; //發布tf坐標變化odom_broadcaster.sendTransform(odom_trans);//載入里程計時間戳odom.header.stamp = ros::Time::now(); //里程計的父子坐標系odom.header.frame_id = "odom";odom.child_frame_id = "base_link"; //里程計位置數據：x,y,z,方向odom.pose.pose.position.x = position_x; odom.pose.pose.position.y = position_y;odom.pose.pose.position.z = 0.0;odom.pose.pose.orientation = odom_quat; //載入線速度和角速度odom.twist.twist.linear.x = vel_linear_x;odom.twist.twist.linear.y = vel_linear_y;odom.twist.twist.angular.z = vel_linear_w; //發布里程計odom_pub.publish(odom); }參考資料
[1] https://github.com/RuPingCen/mick_robot
[2] https://blog.csdn.net/crp997576280/article/details/102026459
[3] https://blog.csdn.net/tiancailx/article/details/78910317
[4] http://wiki.ros.org/tf#static_transform_publisher
上一篇：開源自主導航小車MickX4（二）ROS底盤運動控制 下一篇：開源自主導航小車MickX4（四）底盤URDF模型
歡迎大家點贊在評論區交流討論（cenruping@vip.qq.com） O(∩_∩)O
或者加群交流（1149897304）
總結
以上是生活随笔為你收集整理的开源自主导航小车MickX4（三）底盘ROS节点的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 常见博客程序的市场有这几个
- 下一篇： 中文汉字转换拼音PHP类