STM32端返回的x軸線速度、y軸線速度是相對于自身的機體坐標系的速度,而機器人的位置信息是相對于世界坐標系的位置,所以在對速度進行積分前, 要先將機體坐標系下的x軸線速度、y軸線速度轉換到世界坐標系 ,如圖:
這個坐標變換可以通過一個簡單的旋轉矩陣來實現
其中θ就是機器人的偏航角。相應的程序如下:
/* 對速度進行積分得到位移 */
// 獲取當前時間
current_time = ros::Time::now();
// 獲取積分間隔
double dt = (current_time - last_time).toSec();
last_time = current_time;
// 將機體系速度轉換到里程計坐標系
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
// 速度積分
x += delta_x;
y += delta_y;
在機器人中,一般使用四元數/旋轉矩陣的形式來表示機器人的姿態,而不是歐拉角形式。所以需要將STM32返回的偏航角轉換為四元數,程序如下:
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
以上就獲取了完整的機器人里程計數據,接下來需要將里程計數據發布到ROS中。
nav_msgs::Odometry odom;
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
// 發布坐標變換
odom_broadcaster.sendTransform(odom_trans);
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
// 設置機器人的位置和姿態
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
// 設置機器人的速度
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;
// 發布里程計消息
odom_pub.publish(odom);
運行后,打開PC上的Ubuntu,配置ip從而實現遠程連接嵌入式處理器上的ROS系統,參照:ROS多機通信(https://blog.csdn.net/qq_42688495/article/details/115260247)
配置完成后,重新打開一個終端,輸入:rviz,打開ROS的可視化工具,按照下圖操作即可
可視化結果如下:
最后將該rviz配置保存至文件,點擊File→Save Config As,將配置保存為xxxx.rviz。下次打開時,在命令行運行:rosrun rviz rviz -d xxxx.rviz即可。
-
機器人
+關注
關注
213文章
29735瀏覽量
212856 -
STM32
+關注
關注
2293文章
11032瀏覽量
364849 -
ROS
+關注
關注
1文章
288瀏覽量
17742
發布評論請先 登錄
可視化機器人
關于機器人定位與建圖,這些小技巧你懂嗎
采用MATLAB設計下的可視化與控制
如何去實現一種送餐機器人產品設計
機器人焊裝工位的可視化仿真研究
使用Python可視化數據,機器人開發編程
如何設計液壓式雙足機器人運動數據可視化系統

輪式移動機器人里程計分析
介紹一種基于編碼器合成里程計的方案
ROS機器人導航調參手冊

機器人里程計數據的上傳與接收

評論