9 from teb_local_planner.msg
import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
10 from costmap_converter.msg
import ObstacleArrayMsg
11 from geometry_msgs.msg
import PolygonStamped, Point32, Twist
13 import matplotlib.pyplot
as plotter
20 start_t = rospy.Time.now()
22 point = TrajectoryPointMsg()
23 point.time_from_start = (rospy.Time.now()-start_t)
24 point.velocity.linear.x = data.linear.x
25 point.velocity.linear.y = data.linear.y
26 point.velocity.angular.z = data.angular.z
28 trajectory_gt.append(point)
36 start_t = rospy.Time.now()
38 point = TrajectoryPointMsg()
39 point.time_from_start = (rospy.Time.now()-start_t)
40 if(len(data.obstacles) > 0):
41 point.velocity.linear.x = data.obstacles[0].velocities.twist.linear.x
42 point.velocity.linear.y = data.obstacles[0].velocities.twist.linear.y
43 point.velocity.angular.z = data.obstacles[0].velocities.twist.angular.z
45 trajectory_est.append(point)
49 def plot_velocity_profiles(fig, ax_vx, ax_vy, t_gt, v_x_gt, v_y_gt, omega_gt, t_est, v_x_est, v_y_est, omega_est):
52 ax_vx.set_ylabel(
'Trans. velocity (x) [m/s]')
53 ax_vx.plot(t_gt, v_x_gt,
'-bx', label=
'Ground Truth')
54 ax_vx.plot(t_est, v_x_est,
'-rx', label=
'Estimate')
55 ax_vx.legend(loc=
'upper left')
58 ax_vy.set_ylabel(
'Trans. velocity (y) [m/s]')
59 ax_vy.plot(t_gt, v_y_gt,
'-bx', label=
'Ground Truth')
60 ax_vy.plot(t_est, v_y_est,
'-rx', label=
'Estimate')
61 ax_vy.set_xlabel(
'Time [s]')
70 rospy.init_node(
"visualize_obstacle_velocity_profile", anonymous=
True)
72 topic_name_ground_truth_vel =
"cmd_vel" 73 rospy.Subscriber(topic_name_ground_truth_vel, Twist, twist_callback, queue_size = 1)
74 topic_name_estimated_vel =
"/standalone_converter/costmap_obstacles" 75 rospy.Subscriber(topic_name_estimated_vel, ObstacleArrayMsg, obstacleArrayMsg_callback, queue_size = 1)
77 rospy.loginfo(
"Visualizing ground truth velocity profile published on '%s'.", topic_name_ground_truth_vel)
78 rospy.loginfo(
"Visualizing estimated velocity profile published on '%s'.", topic_name_estimated_vel)
81 fig, (ax_vx, ax_vy) = plotter.subplots(2, sharex=
True)
87 while not rospy.is_shutdown():
99 for point
in trajectory_gt:
100 t_gt.append(point.time_from_start.to_sec())
101 v_x_gt.append(point.velocity.linear.x)
102 v_y_gt.append(point.velocity.linear.y)
103 omega_gt.append(point.velocity.angular.z)
105 for point
in trajectory_est:
106 t_est.append(point.time_from_start.to_sec())
107 v_x_est.append(point.velocity.linear.x)
108 v_y_est.append(point.velocity.linear.y)
109 omega_est.append(point.velocity.angular.z)
112 plot_velocity_profiles(fig, ax_vx, ax_vy, np.asarray(t_gt), np.asarray(v_x_gt), np.asarray(v_y_gt), np.asarray(omega_gt),
113 np.asarray(t_est), np.asarray(v_x_est), np.asarray(v_y_est), np.asarray(omega_est))
117 if __name__ ==
'__main__':
123 except rospy.ROSInterruptException:
def obstacleArrayMsg_callback(data)
def plot_velocity_profiles(fig, ax_vx, ax_vy, t_gt, v_x_gt, v_y_gt, omega_gt, t_est, v_x_est, v_y_est, omega_est)