From 670c2a69981e7c2afbec01549a2d1ce1772c7b66 Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Tue, 1 Oct 2024 04:20:41 +0900 Subject: [PATCH] add an arrow in the tangential direction Signed-off-by: Yoshihiro Kogure --- .../data_collecting_trajectory_publisher.py | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py index 4bbba7c1..d8c396ac 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -1284,6 +1284,53 @@ def timer_callback_traj(self): marker_array.markers.append(marker_traj2) + # [6-2c] add arrow + marker_arrow = Marker() + marker_arrow.type = marker_arrow.ARROW + marker_arrow.id = 2 + marker_arrow.header.frame_id = "map" + + marker_arrow.action = marker_arrow.ADD + + marker_arrow.scale.x = 0.5 + marker_arrow.scale.y = 2.5 + marker_arrow.scale.z = 0.0 + + marker_arrow.color.a = 1.0 + marker_arrow.color.r = 1.0 + marker_arrow.color.g = 0.0 + marker_arrow.color.b = 1.0 + + marker_arrow.lifetime.nanosec = 500000000 + marker_arrow.frame_locked = True + + tangent_vec = np.array( + [ + tmp_traj.points[1].pose.position.x - tmp_traj.points[0].pose.position.x, + tmp_traj.points[1].pose.position.y - tmp_traj.points[0].pose.position.y, + ] + ) + + marker_arrow.points = [] + + start_marker_point = Point() + start_marker_point.x = tmp_traj.points[0].pose.position.x + start_marker_point.y = tmp_traj.points[0].pose.position.y + start_marker_point.z = 0.0 + marker_arrow.points.append(start_marker_point) + + end_marker_point = Point() + end_marker_point.x = tmp_traj.points[0].pose.position.x + 5.0 * tangent_vec[ + 0 + ] / np.linalg.norm(tangent_vec) + end_marker_point.y = tmp_traj.points[0].pose.position.y + 5.0 * tangent_vec[ + 1 + ] / np.linalg.norm(tangent_vec) + end_marker_point.z = 0.0 + marker_arrow.points.append(end_marker_point) + + marker_array.markers.append(marker_arrow) + self.data_collecting_trajectory_marker_array_pub_.publish(marker_array) if debug_matplotlib_plot_flag: