refactor(pcdless_launc/scripts): remove unnecessary scripts (autoware…

…foundation#11)

* remove not useful scripts

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

* rename scripts &  add descriptions

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

* little change

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

* remove odaiba.rviz

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

* grammer fix

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

---------

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

localization/yabloc/pcdless_launch/scripts/extract_rosbag.py → 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)

localization/yabloc/pcdless_launch/scripts/bagplay_for_loc.py → 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 <your_rosbag_path> -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()
 

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()