Moveit 關節控制，資料輸出(.txt)，回放，繪圖,

1.對Moveit進行關節軌迹規劃時，關節的位置position，速度velocity等資料都是不知道的，可以利用監聽相關的話題列印在終端上，相應的指令為：

 $ rostopic echo /joint_states

可以看到joint_states話題裡包含我們需要的關節資訊，

同時，我們也可以編寫一個節點，訂閱joint_states話題，并且将其儲存下來，

在功能包的src下建立一個listener.cpp，具體如下：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/JointState.h"
 
void jointstatesCallback(const sensor_msgs::JointStateConstPtr& msg)
{
  float pos[3],vel[3];
  pos[0]=msg->position[0];
  pos[1]=msg->position[1];
  pos[2]=msg->position[2];
  vel[0]=msg->velocity[0];
  vel[1]=msg->velocity[1];
  vel[2]=msg->velocity[2];
  ROS_INFO("I heard: [%f] [%f] [%f] [%f] [%f] [%f]",pos[0],pos[1],pos[2],vel[0],vel[1],vel[2]);
 
}
 
int main(int argc, char **argv)
{
 
  ros::init(argc, argv, "listener");
 
 
  ros::NodeHandle n;
 
 
  ros::Subscriber sub = n.subscribe("/joint_states", 1000, jointstatesCallback);
 
 
  ros::spin();
 
 
  return 0;
}
           

并且在CMakeList.txt檔案中添加相關的依賴

add_executable(listener src/listener.cpp src/uart.c)

target_link_libraries(listener ${catkin_LIBRARIES})

編譯後運作，

2.利用rqt_plot繪制關節的資訊變化，在Topic中填寫/joint_states/position[0](/position[1])(/position[2])等，并且點選綠色的對号調節橫縱坐标，得到變化明顯。

啟動rqt_plot的指令：rosrun rqt_plot rqt_plot

在ROS系統中，标量資料可以根據消息中提供的時間戳作為時間序列繪制圖形。 

運作可以釋出話題的節點後，可以通過rostopic list檢視目前的釋出的話題，現在，我們不再去使用rostopic echo <topic>，而是通過rxplot指令就能夠看到随時間變化的值的曲線。

rqt_plot /topic/data //單個話題，單個資料

rqt_plot /topic/x:y:z //單個話題，向量資料

rqt_plot /topic/x /topic/y /topic/z

3.将記錄的資料包内容（.bag）轉化為.txt檔案，即：将

file_name.bag

檔案中

topic_name

話題的消息轉換到

Txt_name.txt

檔案中：

rostopic echo -b file_name.bag -p /topic_name > Txt_name.txt
           

其中，file_name.bag為記錄資料的以時間點為名字的資料包（如2018-11-22-11-24.bag），topic_name為節點名稱（如joint_states），Txt_name.txt為想要轉換得到的檔案名，需要自己命名，會自動生成，最終生成.txt檔案需要半分鐘左右，

4..bag檔案生成失敗，隻出現.bag.active檔案

根據目前的經驗，出現這情況是因為電腦在記錄消息時卡掉了，沒有完成最終的錄制。可以轉換成.bag檔案，但是消息有丢失（電腦卡掉後的話題内容） 

恢複：

①切換到”xxx.bag.active”檔案所在的目錄下；

②指令行輸入“rosbag reindex xxx.bag.active”；

③輸入”rosbag fix xxx.bag.active outfile_name.abg”；

注： 

在第二步結束後，除了原來的以.bag.active為字尾的檔案之外，還會生成一個以.bag.org.active為字尾的檔案，注意該檔案隻是中間檔案，第三部輸入的時候不要對該檔案進行修複。

在第三步結束之後，會生成正常的.bag檔案

參考教程：https://blog.csdn.net/weixin_39579805/article/details/78792039

https://blog.csdn.net/Tansir94/article/details/81513517