| 36 | subLidar = rospy.Subscriber("/kloam/mapping/odometry", Odometry, lidar_callback) |
| 37 | rospy.spin() |
| 38 | def plot(folder): |
| 39 | rospy.loginfo("Start plotting odometry") |
| 40 | plt.title('Odometry comparison') |
| 41 | plt.plot(xWheel,yWheel,color='green', label='Wheel encoder') |
| 42 | plt.plot(xSLAM,ySLAM,color='red', label='SLAM odometry') |
| 43 | plt.legend() |
| 44 | plt.xlabel('X (m)') |
| 45 | plt.ylabel('Y (m)') |
| 46 | plt.savefig(folder+"odom.jpg") |
| 47 | plt.show() |
| 48 | rospy.loginfo("Done plotting and saving") |
| 49 | def save(folder): |
| 50 | with open(folder+"odomW.txt",'w') as f1: |
| 51 | for i in range(len(xWheel)): |