From 7326d2773f7378c3951b5d3d51150798a0497657 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Thu, 20 Apr 2023 17:18:09 +0900 Subject: [PATCH] refactor(pcdless_launc/scripts): remove unnecessary scripts (#11) * remove not useful scripts Signed-off-by: Kento Yabuuchi * rename scripts & add descriptions Signed-off-by: Kento Yabuuchi * little change Signed-off-by: Kento Yabuuchi * remove odaiba.rviz Signed-off-by: Kento Yabuuchi * grammer fix Signed-off-by: Kento Yabuuchi --------- Signed-off-by: Kento Yabuuchi --- .../yabloc/pcdless_launch/config/odaiba.rviz | 571 ------------------ .../scripts/control_command_publisher.py | 49 -- ...ct_rosbag.py => extract_rosbag_for_loc.py} | 33 +- .../scripts/initialpose_latch.py | 59 -- ...play_for_loc.py => play_rosbag_for_loc.py} | 26 +- .../pcdless_launch/scripts/plot_navpvt_vel.py | 87 --- .../scripts/publish_control_command.py | 58 ++ ...deo_publisher.py => publish_video_file.py} | 10 +- 8 files changed, 101 insertions(+), 792 deletions(-) delete mode 100644 localization/yabloc/pcdless_launch/config/odaiba.rviz delete mode 100755 localization/yabloc/pcdless_launch/scripts/control_command_publisher.py rename localization/yabloc/pcdless_launch/scripts/{extract_rosbag.py => extract_rosbag_for_loc.py} (66%) delete mode 100755 localization/yabloc/pcdless_launch/scripts/initialpose_latch.py rename localization/yabloc/pcdless_launch/scripts/{bagplay_for_loc.py => play_rosbag_for_loc.py} (79%) delete mode 100755 localization/yabloc/pcdless_launch/scripts/plot_navpvt_vel.py create mode 100755 localization/yabloc/pcdless_launch/scripts/publish_control_command.py rename localization/yabloc/pcdless_launch/scripts/{video_publisher.py => publish_video_file.py} (81%) diff --git a/localization/yabloc/pcdless_launch/config/odaiba.rviz b/localization/yabloc/pcdless_launch/config/odaiba.rviz deleted file mode 100644 index c40c1b16d5010..0000000000000 --- a/localization/yabloc/pcdless_launch/config/odaiba.rviz +++ /dev/null @@ -1,571 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /VectorMap1 - - /VectorMap1/Namespaces1 - - /TF1/Frames1 - Splitter Ratio: 0.6005586385726929 - Tree Height: 741 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" - - Class: initialpose_plugins/InitialPosePanel - Name: InitialPosePanel -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: base_link - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VectorMap - Namespaces: - center_lane_line: true - center_line_arrows: false - crosswalk_lanelets: true - lane_start_bound: true - lanelet direction: false - lanelet_id: false - left_lane_bound: true - pedestrian_marking: true - right_lane_bound: true - road_lanelets: false - stop_lines: true - traffic_light: true - traffic_light_id: false - traffic_light_triangle: false - walkway_lanelets: true - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: pf - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /particle_path - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 239; 41; 41 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: ublox - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/gnss/ublox/path - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - base_link: - Value: true - camera0/camera_link: - Value: true - camera0/camera_optical_link: - Value: true - camera1/camera_link: - Value: true - camera1/camera_optical_link: - Value: true - camera2/camera_link: - Value: true - camera2/camera_optical_link: - Value: true - camera3/camera_link: - Value: true - camera3/camera_optical_link: - Value: true - camera4/camera_link: - Value: true - camera4/camera_optical_link: - Value: true - camera5/camera_link: - Value: true - camera5/camera_optical_link: - Value: true - gnss_link: - Value: true - livox_front_left: - Value: true - livox_front_left_base_link: - Value: true - livox_front_right: - Value: true - livox_front_right_base_link: - Value: true - map: - Value: true - particle_filter: - Value: true - sensing/gnss/ublox/pose: - Value: true - sensor_kit_base_link: - Value: true - tamagawa/imu_link: - Value: true - traffic_light_left_camera/camera_link: - Value: true - traffic_light_left_camera/camera_optical_link: - Value: true - traffic_light_right_camera/camera_link: - Value: true - traffic_light_right_camera/camera_optical_link: - Value: true - velodyne_left: - Value: true - velodyne_left_base_link: - Value: true - velodyne_rear: - Value: true - velodyne_rear_base_link: - Value: true - velodyne_right: - Value: true - velodyne_right_base_link: - Value: true - velodyne_top: - Value: true - velodyne_top_base_link: - Value: true - viewer: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - map: - particle_filter: - base_link: - livox_front_left_base_link: - livox_front_left: - {} - livox_front_right_base_link: - livox_front_right: - {} - sensor_kit_base_link: - camera0/camera_link: - camera0/camera_optical_link: - {} - camera1/camera_link: - camera1/camera_optical_link: - {} - camera2/camera_link: - camera2/camera_optical_link: - {} - camera3/camera_link: - camera3/camera_optical_link: - {} - camera4/camera_link: - camera4/camera_optical_link: - {} - camera5/camera_link: - camera5/camera_optical_link: - {} - gnss_link: - {} - tamagawa/imu_link: - {} - traffic_light_left_camera/camera_link: - traffic_light_left_camera/camera_optical_link: - {} - traffic_light_right_camera/camera_link: - traffic_light_right_camera/camera_optical_link: - {} - velodyne_left_base_link: - velodyne_left: - {} - velodyne_right_base_link: - velodyne_right: - {} - velodyne_top_base_link: - velodyne_top: - {} - velodyne_rear_base_link: - velodyne_rear: - {} - sensing/gnss/ublox/pose: - {} - viewer: - {} - Update Interval: 0 - Value: true - - Class: rviz_common/Group - Displays: - - Alpha: 0.30000001192092896 - Class: rviz_plugins/Image - Enabled: true - Height: 500 - Image Topic Style: true - Left: 0 - Name: Overlay - Top: 0 - Topic: /overlay_image - Value: true - Width: 800 - - Alpha: 0.30000001192092896 - Class: rviz_plugins/Image - Enabled: true - Height: 300 - Image Topic Style: true - Left: 300 - Name: ProjectedLSD - Top: 500 - Topic: /projected_image - Value: true - Width: 300 - - Alpha: 0.30000001192092896 - Class: rviz_plugins/Image - Enabled: true - Height: 300 - Image Topic Style: true - Left: 0 - Name: LSD - Top: 500 - Topic: /lsd_image - Value: true - Width: 300 - - Alpha: 0.10000000149011612 - Class: rviz_plugins/Image - Enabled: true - Height: 200 - Image Topic Style: true - Left: 600 - Name: CostMap - Top: 500 - Topic: /match_image - Value: true - Width: 200 - - Alpha: 0.10000000149011612 - Class: rviz_plugins/Image - Enabled: true - Height: 300 - Image Topic Style: true - Left: 0 - Name: Segment - Top: 800 - Topic: /segmented_image - Value: true - Width: 300 - Enabled: true - Name: Overlay - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Particles - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /weighted_particles/marker_array - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: ProjectedLsd - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /marker - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: GnssEffectRange - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /gnss/effect_marker - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: CostMapRange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /cost_map_range - Value: false - - Class: rviz_common/Group - Displays: - - Background Alpha: 0 - Background Color: 50; 50; 50 - Class: rviz_plugins/String - Enabled: true - Font Size: 16 - Foreground Color: 255; 255; 40 - Foregrund Alpha: 0.800000011920929 - Height: 256 - Left: 800 - Name: KalmanStatus - Top: 0 - Topic: /kalman/status - Value: true - Width: 400 - - Background Alpha: 0 - Background Color: 50; 50; 50 - Class: rviz_plugins/String - Enabled: true - Font Size: 16 - Foreground Color: 255; 255; 40 - Foregrund Alpha: 0.800000011920929 - Height: 256 - Left: 1200 - Name: GroundStatus - Top: 0 - Topic: /ground_status - Value: true - Width: 400 - - Background Alpha: 0 - Background Color: 50; 50; 50 - Class: rviz_plugins/String - Enabled: true - Font Size: 16 - Foreground Color: 255; 255; 40 - Foregrund Alpha: 0.800000011920929 - Height: 256 - Left: 1200 - Name: CovarianceStatus - Top: 150 - Topic: /covariance_diag - Value: true - Width: 400 - - Background Alpha: 0 - Background Color: 50; 50; 50 - Class: rviz_plugins/String - Enabled: true - Font Size: 16 - Foreground Color: 255; 255; 40 - Foregrund Alpha: 0.800000011920929 - Height: 256 - Left: 800 - Name: ApeStatus - Top: 200 - Topic: /ape_diag - Value: true - Width: 400 - Enabled: true - Name: Status - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SignMarker - Namespaces: - virtual: true - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sign_board_marker - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: NearLL2Point - Position Transformer: XYZ - Selectable: true - Size (Pixels): 6 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /near_cloud - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: 0.5449999570846558 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 11.181879997253418 - Target Frame: base_link - Value: TopDownOrtho (rviz_default_plugins) - X: -9.702136039733887 - Y: 9.046465873718262 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 1043 - Hide Left Dock: true - Hide Right Dock: true - InitialPosePanel: - collapsed: false - PlayerPanel: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b900000370fc020000000bfb000000100044006900730070006c006100790073000000003d00000370000000c900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000033e000002610000000000000000fb0000000a0049006d00610067006500000001f8000001ba0000000000000000fb0000000a0049006d00610067006501000003ae000001f1000000000000000000000001000002d100000370fc0200000003fb0000000a00560069006500770073000000003d00000370000000a400fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000043fc0100000004fb000000160050006c006100790065007200500061006e0065006c0100000000000000850000008500fffffffb0000000800540069006d0065010000008b00000457000002eb00fffffffb000000200049006e0069007400690061006c0050006f0073006500500061006e0065006c01000004e800000298000001ec00fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000037000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 2560 - Y: 0 diff --git a/localization/yabloc/pcdless_launch/scripts/control_command_publisher.py b/localization/yabloc/pcdless_launch/scripts/control_command_publisher.py deleted file mode 100755 index f06c7ea0449ba..0000000000000 --- a/localization/yabloc/pcdless_launch/scripts/control_command_publisher.py +++ /dev/null @@ -1,49 +0,0 @@ -#!/usr/bin/python3 -import rclpy -from rclpy.node import Node -from autoware_auto_control_msgs.msg import AckermannControlCommand -from std_msgs.msg import String -from std_msgs.msg import Float32 -import numpy as np - -class control_command_publisher(Node): - def __init__(self): - super().__init__('control_command_publisher') - - self.sub_command_ = self.create_subscription( - AckermannControlCommand, '/control/command/control_cmd', self.command_callback, 10) - - self.pub_string_ = self.create_publisher(String, '/string/command', 10) - self.pub_float_ = self.create_publisher(Float32, '/float/steer', 10) - - def command_callback(self, msg: AckermannControlCommand): - - commands={} - commands['steer_angle(deg)']=msg.lateral.steering_tire_angle * 180/np.pi - commands['steer_rate(deg/s)']=msg.lateral.steering_tire_rotation_rate * 180/np.pi - commands['speed']=msg.longitudinal.speed - commands['accel']=msg.longitudinal.acceleration - commands['jerk ']=msg.longitudinal.jerk - - str_msg=String() - str_msg.data='-- Control Command Status --\n' - for key,value in commands.items(): - str_msg.data+= key+': '+'{:.3f}'.format(value)+'\n' - - self.pub_string_.publish(str_msg) - - float_msg=Float32() - float_msg.data=msg.lateral.steering_tire_angle * 180/np.pi - self.pub_float_.publish(float_msg) - -def main(args=None): - rclpy.init(args=args) - - converter = control_command_publisher() - rclpy.spin(converter) - converter.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/localization/yabloc/pcdless_launch/scripts/extract_rosbag.py b/localization/yabloc/pcdless_launch/scripts/extract_rosbag_for_loc.py similarity index 66% rename from localization/yabloc/pcdless_launch/scripts/extract_rosbag.py rename to localization/yabloc/pcdless_launch/scripts/extract_rosbag_for_loc.py index e7954710819f2..cf127d9a93f2c 100755 --- a/localization/yabloc/pcdless_launch/scripts/extract_rosbag.py +++ b/localization/yabloc/pcdless_launch/scripts/extract_rosbag_for_loc.py @@ -1,4 +1,9 @@ -#!/usr/bin/python3 +#!/usr/bin/env python3 +''' +This script extracts only the topics necessary for localization from big rosbag. + +e.g. $ ./extract_rosabg_for_loc.py -h +''' import glob import subprocess import shutil @@ -11,11 +16,8 @@ '/sensing/gnss/ublox/nav_sat_fix', '/sensing/gnss/ublox/navpvt', '/sensing/imu/tamagawa/imu_raw', - '/vehicle/status/actuation_status', # iv, universe '/vehicle/status/twist', # iv '/vehicle/status/velocity_status', # universe - '/vehicle/status/steering', # iv - '/vehicle/status/steering_status', # universe ] LIDAR_TOPICS = [ @@ -40,10 +42,14 @@ def extractIndex(name): def main(): parser = argparse.ArgumentParser() - parser.add_argument('-b', '--basic', action='store_true', help='include basic topics') - parser.add_argument('-l', '--lidar', action='store_true', help='include lidar topics') - parser.add_argument('-c', '--camera', action='store_true', help='include camera topics') - parser.add_argument('-o', '--output', default='filtered', help='include camera topics') + parser.add_argument('-b', '--basic', action='store_true', + help='include basic topics') + parser.add_argument('-l', '--lidar', action='store_true', + help='include lidar topics') + parser.add_argument('-c', '--camera', action='store_true', + help='include camera topics') + parser.add_argument('-o', '--output', default='filtered', + help='include camera topics') args = parser.parse_args() TOPICS = [] @@ -55,7 +61,8 @@ def main(): TOPICS.extend(LIDAR_TOPICS) if len(TOPICS) == 0: - print_highlight('You have to choice at least on topics among `basic`, `lidar`, `camera`') + print_highlight( + 'You have to choice at least on topics among `basic`, `lidar`, `camera`') parser.print_help() return @@ -68,11 +75,11 @@ def main(): indexed_files.sort(key=lambda x: x[0]) # Filter necessary topics - print_highlight('Process '+str(len(indexed_files))+' files.') + print_highlight('Process ' + str(len(indexed_files)) + ' files.') dst_files = [] for index, f in indexed_files: - print_highlight(str(index)+' '+f) - tmp_dst = 'tmp'+str(index) + print_highlight(str(index) + ' ' + f) + tmp_dst = 'tmp' + str(index) dst_files.append(tmp_dst) command = ['ros2', 'bag', 'filter', f, '-o', tmp_dst, '-i'] for t in TOPICS: @@ -81,7 +88,7 @@ def main(): # Merge tmp files print_highlight('Merge filtered files.') - command = ['ros2', 'bag', 'merge', '-o', args.output] + command = ['ros2', 'bag', 'merge', '-o', args.output] for f in dst_files: command.append(f) subprocess.run(command) diff --git a/localization/yabloc/pcdless_launch/scripts/initialpose_latch.py b/localization/yabloc/pcdless_launch/scripts/initialpose_latch.py deleted file mode 100755 index b28a185afb01d..0000000000000 --- a/localization/yabloc/pcdless_launch/scripts/initialpose_latch.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/python3 -from rclpy.time import Time -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import PoseWithCovarianceStamped -from rclpy.parameter import Parameter - - -class InitialPoseLatch(Node): - def __init__(self): - super().__init__('initial_pose_latch') - - self.sub_pose_ = self.create_subscription( - PoseWithCovarianceStamped, '/initialpose', self.pose_callback, 10) - - self.pub_pose_ = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 10) - - self.set_parameters([Parameter('use_sim_time', Parameter.Type.BOOL, True)]) - - self.last_initial_psoe_and_time = None - self.timer_ = self.create_timer(1.0, self.timer_callback, clock=self.get_clock()) - self.last_time_diff = None - - def timer_callback(self): - if self.last_initial_psoe_and_time is None: - return - - now_stamp = self.get_clock().now() - target = self.last_initial_psoe_and_time[0] - dt = abs(now_stamp.nanoseconds-target.nanoseconds)/1e9 - - if self.last_time_diff is not None: - if dt < self.last_time_diff: - self.get_logger().info('It will publish initialpose soon ' + str(dt)) - - if dt < 0.55: - self.get_logger().info('Publish initialpose') - self.pub_pose_.publish(self.last_initial_psoe_and_time[1]) - - self.last_time_diff = dt - - def pose_callback(self, msg: PoseWithCovarianceStamped): - stamp = msg.header.stamp - stamp = Time(seconds=stamp.sec, nanoseconds=stamp.nanosec) - self.get_logger().info('Subscribed initialpose') - self.last_initial_psoe_and_time = (stamp, msg) - - -def main(args=None): - rclpy.init(args=args) - - initialpose_latch = InitialPoseLatch() - rclpy.spin(initialpose_latch) - initialpose_latch.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/localization/yabloc/pcdless_launch/scripts/bagplay_for_loc.py b/localization/yabloc/pcdless_launch/scripts/play_rosbag_for_loc.py similarity index 79% rename from localization/yabloc/pcdless_launch/scripts/bagplay_for_loc.py rename to localization/yabloc/pcdless_launch/scripts/play_rosbag_for_loc.py index 0d28b8722172f..cb3c4987eedfd 100755 --- a/localization/yabloc/pcdless_launch/scripts/bagplay_for_loc.py +++ b/localization/yabloc/pcdless_launch/scripts/play_rosbag_for_loc.py @@ -1,4 +1,9 @@ -#!/usr/bin/python3 +#!/usr/bin/env python3 +''' +Script to publish only the topics necessary to make the localization component work from rosbag. + +e.g. $ ./play_rosbag_for_loc.py -o +''' import os import subprocess import argparse @@ -13,14 +18,10 @@ '/sensing/gnss/ublox/nav_sat_fix', '/sensing/gnss/ublox/navpvt', '/sensing/imu/tamagawa/imu_raw', - '/sensing/lidar/left/velodyne_packets', - '/sensing/lidar/right/velodyne_packets', - '/sensing/lidar/rear/velodyne_packets', '/sensing/lidar/top/velodyne_packets', '/sensing/camera/traffic_light/camera_info', '/sensing/camera/traffic_light/image_raw/compressed', - '/vehicle/status/gear_status', - '/vehicle/status/steering_status', + # .universe '/vehicle/status/velocity_status', # .iv '/vehicle/status/twist', @@ -41,7 +42,8 @@ def doesRosbagIncludeTopics(rosbag): reader = SequentialReader() bag_storage_otions = StorageOptions(uri=rosbag, storage_id="sqlite3") - bag_converter_options = ConverterOptions(input_serialization_format="cdr", output_serialization_format="cdr") + bag_converter_options = ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr") reader.open(bag_storage_otions, bag_converter_options) included = [] @@ -57,13 +59,13 @@ def printCommand(command): for c in command[0:idx]: print(c, end=' ') print(command[idx]) - for c in command[idx+1:]: + for c in command[idx + 1:]: print('\t', c) print('\033[0m') print('The following topics are not included in rosbag') for t in TOPICS: - if not t in command[idx+1:]: + if not t in command[idx + 1:]: print('\t', t) @@ -81,8 +83,10 @@ def makeOverrideYaml(yaml_path): def main(): parser = argparse.ArgumentParser() parser.add_argument('rosbag', help='rosbag file to replay') - parser.add_argument('-r', '--rate', default='1.0', help='rate at which to play back messages. Valid range > 0.0.') - parser.add_argument('-o', '--override', action='store_true', help='qos profile overrides') + parser.add_argument('-r', '--rate', default='1.0', + help='rate at which to play back messages. Valid range > 0.0.') + parser.add_argument('-o', '--override', + action='store_true', help='qos profile overrides') args = parser.parse_args() diff --git a/localization/yabloc/pcdless_launch/scripts/plot_navpvt_vel.py b/localization/yabloc/pcdless_launch/scripts/plot_navpvt_vel.py deleted file mode 100755 index 34d6a88c21997..0000000000000 --- a/localization/yabloc/pcdless_launch/scripts/plot_navpvt_vel.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/python3 -from rosidl_runtime_py.utilities import get_message -from rclpy.serialization import deserialize_message -import sys -from rosbag2_py import SequentialReader -from rosbag2_py import StorageOptions -from rosbag2_py import ConverterOptions -import matplotlib.pyplot as plt -import numpy as np - - -class FixChecker(): - def __init__(self, bag_file): - self.reader = SequentialReader() - bag_storage_otions = StorageOptions(uri=bag_file, storage_id="sqlite3") - bag_converter_options = ConverterOptions(input_serialization_format="cdr", output_serialization_format="cdr") - self.reader.open(bag_storage_otions, bag_converter_options) - - self.type_map = {} - for topic_type in self.reader.get_all_topics_and_types(): - self.type_map[topic_type.name] = topic_type.type - - def __checkNavPvt(self): - vels = [] - first_stamp = None - while self.reader.has_next(): - (topic, data, stamp) = self.reader.read_next() - if topic != '/sensing/gnss/ublox/navpvt': - continue - - if first_stamp is None: - first_stamp = stamp - - msg_type = get_message(self.type_map[topic]) - msg = deserialize_message(data, msg_type) - - dt = (stamp-first_stamp)*1e-9 - if dt < 300 or dt > 520: - continue - vels.append([dt, msg.vel_n*1e-3, msg.vel_e*1e-3, msg.vel_d*1e-3]) - - vels = np.array(vels) - plt.grid() - plt.plot(vels[:, 0], vels[:, 1], color='r') - plt.plot(vels[:, 0], vels[:, 2], color='g') - plt.plot(vels[:, 0], vels[:, 3], color='b') - plt.show() - - def __call__(self): - vels = [] - first_stamp = None - while self.reader.has_next(): - (topic, data, stamp) = self.reader.read_next() - if topic != '/sensing/gnss/ublox/navpvt': - continue - - if first_stamp is None: - first_stamp = stamp - - msg_type = get_message(self.type_map[topic]) - msg = deserialize_message(data, msg_type) - - dt = (stamp-first_stamp)*1e-9 - if dt<300 or dt > 600: - continue - vels.append([dt, msg.vel_n, msg.vel_e,msg.vel_d]) - - vels = np.array(vels) - print(vels) - plt.grid() - plt.plot(vels[:, 0], vels[:, 1], color='r') - plt.plot(vels[:, 0], vels[:, 2], color='g') - plt.plot(vels[:, 0], vels[:, 3], color='b') - plt.show() - - -def main(): - if len(sys.argv) == 1: - return - bag_file = sys.argv[1] - - checker = FixChecker(bag_file) - checker() - - -if __name__ == "__main__": - main() diff --git a/localization/yabloc/pcdless_launch/scripts/publish_control_command.py b/localization/yabloc/pcdless_launch/scripts/publish_control_command.py new file mode 100755 index 0000000000000..3ee38dc3a049a --- /dev/null +++ b/localization/yabloc/pcdless_launch/scripts/publish_control_command.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 +''' +This is a script to visualize the control topic of Autoware to validate planning/control component. +The node subscribes /control/command/control_cmd and publishes it as string and floot value. + +e.g. $ ./publish_control_command.py +''' +import rclpy +from rclpy.node import Node +from autoware_auto_control_msgs.msg import AckermannControlCommand +from std_msgs.msg import String +from std_msgs.msg import Float32 +import numpy as np + + +class control_command_publisher(Node): + def __init__(self): + super().__init__('control_command_publisher') + + self.sub_command_ = self.create_subscription( + AckermannControlCommand, '/control/command/control_cmd', self.command_callback, 10) + + self.pub_string_ = self.create_publisher(String, '/string/command', 10) + self.pub_float_ = self.create_publisher(Float32, '/float/steer', 10) + + def command_callback(self, msg: AckermannControlCommand): + + commands = {} + commands['steer_angle(deg)'] = msg.lateral.steering_tire_angle * \ + 180 / np.pi + commands['steer_rate(deg/s)'] = msg.lateral.steering_tire_rotation_rate * 180 / np.pi + commands['speed'] = msg.longitudinal.speed + commands['accel'] = msg.longitudinal.acceleration + commands['jerk '] = msg.longitudinal.jerk + + str_msg = String() + str_msg.data = '-- Control Command Status --\n' + for key, value in commands.items(): + str_msg.data += key + ': ' + '{:.3f}'.format(value) + '\n' + + self.pub_string_.publish(str_msg) + + float_msg = Float32() + float_msg.data = msg.lateral.steering_tire_angle * 180 / np.pi + self.pub_float_.publish(float_msg) + + +def main(args=None): + rclpy.init(args=args) + + converter = control_command_publisher() + rclpy.spin(converter) + converter.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/localization/yabloc/pcdless_launch/scripts/video_publisher.py b/localization/yabloc/pcdless_launch/scripts/publish_video_file.py similarity index 81% rename from localization/yabloc/pcdless_launch/scripts/video_publisher.py rename to localization/yabloc/pcdless_launch/scripts/publish_video_file.py index d6dd538c953d2..ad89305865cf5 100755 --- a/localization/yabloc/pcdless_launch/scripts/video_publisher.py +++ b/localization/yabloc/pcdless_launch/scripts/publish_video_file.py @@ -1,4 +1,9 @@ -#!/usr/bin/python3 +#!/usr/bin/env python3 +''' +This node reads a video file and publishes it as an image topic. + +e.g. publish_video_file.py video.mp4 +''' from sensor_msgs.msg import Image from rclpy.node import Node import rclpy @@ -10,7 +15,8 @@ class Mp4Publisher(Node): def __init__(self, mp4_file): super().__init__('mp4_publisher') - self.publisher_ = self.create_publisher(Image, '/sensing/camera/undistorted/image_raw', 10) + self.publisher_ = self.create_publisher( + Image, '/sensing/camera/undistorted/image_raw', 10) self.timer_ = self.create_timer(0.1, self.timer_callback) self.video_ = cv2.VideoCapture(mp4_file) self.bridge_ = CvBridge()