Commit 2687e10e authored by Kay's avatar Kay
Browse files

Update jupyter plotting tool

parent a2dbd31c
...@@ -32,20 +32,21 @@ ...@@ -32,20 +32,21 @@
bag = rosbag.Bag(bag_name) bag = rosbag.Bag(bag_name)
error = [] error = []
poses = [] poses = []
speeds = [] speeds = []
path = None path = None
for topic, msg, t in bag.read_messages(topics=['/controller/cte', '/sim_car_pose/pose', '/controller/path/poses', '/vesc/high_level/ackermann_cmd_mux/input/nav_0']): for topic, msg, t in bag.read_messages(topics=['/controller/cte', '/car_pose', '/controller/path/poses', '/vesc/high_level/ackermann_cmd_mux/input/nav_0']):
if 'cte' in topic: if '/controller/cte' in topic:
error.append(msg.data) error.append(msg.data)
if 'sim' in topic and path is not None: if '/car_pose' in topic and path is not None:
pose = pose_to_config(msg.pose) pose = pose_to_config(msg.pose)
poses.append(pose) poses.append(pose)
if 'nav' in topic and path is not None:
speeds.append(msg.drive.speed)
if '/controller/path/poses' in topic: if '/controller/path/poses' in topic:
path = map(pose_to_config, msg.poses) path = map(pose_to_config, msg.poses)
if '/vesc/high_level/ackermann_cmd_mux/input/nav_0' in topic and path is not None:
speeds.append(msg.drive.speed)
plt.figure(1) plt.figure(1)
plt.subplot(221) plt.subplot(221)
plt.plot(error) plt.plot(error)
plt.axhline(0, color='grey') plt.axhline(0, color='grey')
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment