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

Moveit 关节控制，数据输出(.txt)，回放，绘图,

猜你喜欢