From 4651cb9c34665f393ca28e9c5cbd32559430a27d Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Tue, 15 Aug 2023 14:52:23 +0200 Subject: [PATCH] Own stuff working --- .gitignore | 1 + .../extract_images_and_labels.py | 209 +++++++++----- wild_visual_navigation/utils/dataset_info.py | 259 +++++++++++------- .../launch/debug/param.launch | 7 + 4 files changed, 301 insertions(+), 175 deletions(-) diff --git a/.gitignore b/.gitignore index 133a267b..2af232ec 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ wild_visual_navigation/notebooks* results/* assets/dino/* assets/stego/cocostuff27_vit_base_5.ckpt +assets/weights/* .pytest_cache/** .vscode/** notebooks/** diff --git a/scripts/dataset_generation/extract_images_and_labels.py b/scripts/dataset_generation/extract_images_and_labels.py index 1c2702b2..62c90a7f 100644 --- a/scripts/dataset_generation/extract_images_and_labels.py +++ b/scripts/dataset_generation/extract_images_and_labels.py @@ -9,7 +9,7 @@ from tf_bag import BagTfTransformer import rospy import rosparam -from sensor_msgs.msg import Image, CameraInfo +from sensor_msgs.msg import Image, CameraInfo, CompressedImage import rosbag from postprocessing_tools_ros.merging import merge_bags_single, merge_bags_all @@ -64,29 +64,63 @@ def do(n, dry_run): s = os.path.join(ROOT_DIR, d["name"]) - valid_topics = ["/state_estimator/anymal_state", "/alphasense_driver_ros/cam4", "/log/state/desiredRobotTwist"] + # For South Africa + # valid_topics = ["/state_estimator/anymal_state", "/alphasense_driver_ros/cam4/debayered/compressed", + # "/twist_mux/twist"] + # valid_topics = ["/state_estimator/anymal_state", "/alphasense_driver_ros/cam4/debayered", + # "/twist_mux/twist"] + # valid_topics = ["/state_estimator/anymal_state", "/alphasense_driver_ros/cam4/debayered"] + # valid_topics = ["/state_estimator/anymal_state", "/alphasense_driver_ros/cam4/debayered"] + + valid_topics = ["/state_estimator/anymal_state", "/wide_angle_camera_front/img_out"] + + # valid_topics = ["/state_estimator/anymal_state", "/alphasense_driver_ros/cam4/dropped/debayered/slow"] # Merge rosbags if necessary - rosbags = [ - str(s) - for s in Path(s).rglob("*.bag") - if str(s).find("lpc_robot_state") != -1 - or str(s).find("jetson_images") != -1 - or str(s).find("lpc_locomotion") != -1 - ] - try: - rosbags.sort(key=lambda x: int(x.split("/")[-1][-5])) - except: - pass + # rosbags = [ + # str(s) + # for s in Path(s).rglob("*.bag") + # if str(s).find("lpc_robot_state") != -1 # Contains /state_estimator/anymal_state + # or str(s).find("jetson_images") != -1 # Contains /alphasense_driver_ros/cam4/debayered/compressed + # # or str(s).find("lpc_locomotion") != -1 # Contains /twist_mux/twist + # ] + # try: + # rosbags.sort(key=lambda x: int(x.split("/")[-1][-5])) + # except: + # pass + + # rosbags = ["/home/rschmid/RosBags/perugia_grass_bags/raw/lpc_robot_state_0.bag", + # "/home/rschmid/RosBags/perugia_grass_bags/raw/lpc_robot_state_1.bag", + # "/home/rschmid/RosBags/perugia_grass_bags/raw/lpc_robot_state_2.bag", + # "/home/rschmid/RosBags/perugia_grass_bags/raw/jetson_images.bag"] + # rosbags = ["/home/rschmid/RosBags/perugia/lpc_robot_state_0.bag", + # "/home/rschmid/RosBags/perugia/jetson_images_0.bag"] + rosbags = ["/home/rschmid/RosBags/6/images.bag", + "/home/rschmid/RosBags/6/2023-03-02-11-13-08_anymal-d020-lpc_mission_0.bag", + "/home/rschmid/RosBags/6/2023-03-02-11-13-08_anymal-d020-lpc_mission_1.bag"] + # rosbags = ["/home/rschmid/RosBags/4/images.bag", + # "/home/rschmid/RosBags/4/2023-03-02-10-11-56_anymal-d020-lpc_mission_0.bag", + # "/home/rschmid/RosBags/4/2023-03-02-10-11-56_anymal-d020-lpc_mission_1.bag"] + # rosbags = ["/home/rschmid/RosBags/11/images.bag", + # "/home/rschmid/RosBags/11/2023-03-02-12-31-23_anymal-d020-lpc_mission_0.bag", + # "/home/rschmid/RosBags/11/2023-03-02-12-31-23_anymal-d020-lpc_mission_1.bag", + # "/home/rschmid/RosBags/11/2023-03-02-12-31-23_anymal-d020-lpc_mission_2.bag"] + + # rosbags = ["/home/rschmid/RosBags/hoengg_extract/2021-12-06-15-31-17.bag", + # "/home/rschmid/RosBags/hoengg_extract/output_2023-02-10-15-20-27.bag"] output_bag_wvn = s + "_wvn.bag" output_bag_tf = s + "_tf.bag" - tf_bags = [b for b in rosbags if b.find("lpc_robot_state") != -1] + # tf_bags = [b for b in rosbags if b.find("lpc_robot_state") != -1] # jetson locomotion robot + # if not os.path.exists(output_bag_tf): + # total_included_count, total_skipped_count = merge_bags_single( + # input_bag=tf_bags, output_bag=output_bag_tf, topics="/tf /tf_static", verbose=True + # ) if not os.path.exists(output_bag_tf): total_included_count, total_skipped_count = merge_bags_single( - input_bag=tf_bags, output_bag=output_bag_tf, topics="/tf /tf_static", verbose=True + input_bag=rosbags, output_bag=output_bag_tf, topics="/tf /tf_static", verbose=True ) if not os.path.exists(output_bag_wvn): total_included_count, total_skipped_count = merge_bags_single( @@ -99,21 +133,22 @@ def do(n, dry_run): mission = s.split("/")[-1] # 2022-05-12T11:56:13_mission_0_day_3 - # extraction_store_folder = f"/media/Data/Datasets/2022_Perugia/wvn_output/day3/{mission}" - extraction_store_folder = f"/media/matias/datasets/2022_Perugia/wvn_output/day3/{mission}" + # running_store_folder = f"/media/Data/Datasets/2022_Perugia/wvn_output/day3/{mission}" + running_store_folder = f"/home/rschmid/RosBags/output/{mission}" - if os.path.exists(extraction_store_folder): - print(f"Stopped because folder already exists: {extraction_store_folder}") - return + if os.path.exists(running_store_folder): + print("Folder already exists, but proceeding!") + # return rosparam.set_param("wild_visual_navigation_node/mode", "extract_labels") - rosparam.set_param("wild_visual_navigation_node/extraction_store_folder", extraction_store_folder) + # rosparam.set_param("wild_visual_navigation_node/running_store_folder", running_store_folder) + rosparam.set_param("wild_visual_navigation_node/running_store_folder", running_store_folder) # for proprioceptive callback state_msg_valid = False desired_twist_msg_valid = False - pub = rospy.Publisher("/alphasense_driver_ros/cam4", Image, queue_size=1) + # pub = rospy.Publisher("/wide_angle_camera_front/image_color/compressed", CompressedImage, queue_size=1) wvn_ros_interface = WvnRosInterface() print("-" * 80) @@ -122,27 +157,46 @@ def do(n, dry_run): wvn_ros_interface.setup_rosbag_replay(tf_listener) print("done loading tf") + # Höngg new info_msg = CameraInfo() - info_msg.height = 540 - info_msg.width = 720 - info_msg.distortion_model = "plumb_bob" - info_msg.K = [347.548139773951, 0.0, 342.454373227748, 0.0, 347.434712422309, 271.368057185649, 0.0, 0.0, 1.0] - info_msg.P = [ - 347.548139773951, - 0.0, - 342.454373227748, - 0.0, - 0.0, - 347.434712422309, - 271.368057185649, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - ] + info_msg.height = 1080 + info_msg.width = 1440 + info_msg.distortion_model = "equidistant" + info_msg.D = [0.4316922809468283, 0.09279900476637248, -0.4010909691803734, 0.4756163338479413] + info_msg.K = [575.6050407221768, 0.0, 745.7312198525915, 0.0, 578.564849365178, 519.5207040671075, 0.0, 0.0, 1.0] + info_msg.P = [575.6050407221768, 0.0, 745.7312198525915, 0.0, 0.0, 578.564849365178, 519.5207040671075, 0.0, 0.0, 0.0, 1.0, 0.0] info_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + # Perugia + # info_msg = CameraInfo() + # info_msg.height = 540 + # info_msg.width = 720 + # info_msg.distortion_model = "plumb_bob" + # info_msg.K = [347.548139773951, 0.0, 342.454373227748, 0.0, 347.434712422309, 271.368057185649, 0.0, 0.0, 1.0] + # info_msg.P = [347.548139773951, 0.0, 342.454373227748, 0.0, 0.0, 347.434712422309, 271.368057185649, 0.0, + # 0.0, 0.0, 1.0, 0.0] + # info_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + + # Höngg + # info_msg = CameraInfo() + # info_msg.height = 540 + # info_msg.width = 720 + # info_msg.distortion_model = "equidistant" + # info_msg.K = [349.5636550689, 0.0, 357.2746308879, 0.0, 349.4046775293, 264.3108985411, 0.0, 0.0, 1.0] + # info_msg.P = [349.5636550689, 0.0, 357.2746308879, 0.0, 0.0, 349.4046775293, 264.3108985411, 0.0, 0.0, 0.0, 1.0, 0.0] + # info_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + + # South Africa + # info_msg = CameraInfo() + # info_msg.height = 1080 + # info_msg.width = 1440 + # info_msg.distortion_model = "equidistant" + # info_msg.D = [-0.0480706813, 0.0129997684, -0.0112199955, 0.0026955514] + # info_msg.K = [699.2284099702, 0.0, 711.8009584441, 0.0, 698.546880367, 524.7993478318, 0.0, 0.0, 1.0] + # info_msg.P = [699.2284099702, 0.0, 711.8009584441, 0.0, 0.0, 698.546880367, 524.7993478318, 0.0, 0.0, + # 0.0, 1.0, 0.0] + # info_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + rosbag_info_dict = get_bag_info(output_bag_wvn) total_msgs = sum([x["messages"] for x in rosbag_info_dict["topics"] if x["topic"] in valid_topics]) total_time_img = 0 @@ -160,54 +214,65 @@ def do(n, dry_run): bar_format="{desc:<13}{percentage:3.0f}%|{bar:20}{r_bar}", ) as pbar: for (topic, msg, ts) in bag.read_messages(topics=None, start_time=start_time, end_time=end_time): + + if rospy.is_shutdown(): + return pbar.update(1) st = time.time() + # print(topic) if topic == "/state_estimator/anymal_state": + # print("Received /state_estimator/anymal_state") state_msg = anymal_msg_callback(msg, return_msg=True) state_msg_valid = True - elif topic == "/log/state/desiredRobotTwist": - desired_twist_msg = msg - desired_twist_msg_valid = True - - elif topic == "/alphasense_driver_ros/cam4": - N = 1000 - for i in range(N): - pub.publish(msg) - # Change this for service call - try: - image_msg = rospy.wait_for_message( - "/alphasense_driver_ros/cam4/debayered", Image, timeout=0.01 - ) - suc = True - except Exception as e: - suc = False - pass - if suc: - if msg.header.stamp == image_msg.header.stamp: - break - if i >= N - 1: - raise Exception("Timeout waiting for debayered image message") + # elif topic == "/twist_mux/twist": + # # print("Received /twist_mux/twist") + # desired_twist_msg = msg + # desired_twist_msg_valid = True + + elif topic == "/wide_angle_camera_front/img_out": + image_msg = msg + # Image is already debayered, need to recompress it + print("Received /wide_angle_camera_front/img_out") + # for i in range(100): + # pub.publish(msg) + # try: + # image_msg = rospy.wait_for_message( + # "/wide_angle_camera_front/image_color", Image, timeout=0.1 + # ) + # suc = True + # except: + # suc = False + # pass + # if suc: + # if msg.header.stamp == image_msg.header.stamp: + # break + # if i >= 99: + # raise Exception("Timeout waiting for debayered image message") info_msg.header = msg.header + camera_options = {} + camera_options['name'] = "wide_angle_camera_front" + camera_options["use_for_training"] = True + try: - wvn_ros_interface.image_callback(image_msg, info_msg) + wvn_ros_interface.image_callback(image_msg, info_msg, camera_options) except Exception as e: - tqdm.write("Bad image_callback", e) + print("Bad image_callback", e) total_time_img += time.time() - st # print(f"image time: {total_time_img} , state time: {total_time_state}") - tqdm.write("add image") - if state_msg_valid and desired_twist_msg_valid: + print("add image") + if state_msg_valid: try: - wvn_ros_interface.robot_state_callback(state_msg, desired_twist_msg) + wvn_ros_interface.robot_state_callback(state_msg, None) except Exception as e: - tqdm.write("Bad robot_state callback ", e) + print("Bad robot_state callback ", e) state_msg_valid = False - desired_twist_msg_valid = True + # desired_twist_msg_valid = True total_time_state += time.time() - st - tqdm.write("add supervision") + # print("add supervision") print("Finished with converting the dataset") rospy.signal_shutdown("stop the node") @@ -217,8 +282,8 @@ def do(n, dry_run): import argparse parser = argparse.ArgumentParser() - parser.add_argument("--n", type=int, default=2, help="Store data") + parser.add_argument("--n", type=int, default=0, help="Store data") parser.add_argument("--dry_run", type=int, default=0, help="Store data") args = parser.parse_args() - print(args.n) + do(args.n, args.dry_run) diff --git a/wild_visual_navigation/utils/dataset_info.py b/wild_visual_navigation/utils/dataset_info.py index 2d680f85..e186b9c1 100644 --- a/wild_visual_navigation/utils/dataset_info.py +++ b/wild_visual_navigation/utils/dataset_info.py @@ -6,116 +6,169 @@ """ # ROOT_DIR = "/media/Data/Datasets/2022_Perugia" -ROOT_DIR = "/media/matias/datasets/2022_Perugia" +# ROOT_DIR = "/home/rschmid/RosBags/South_Africa2" +# perugia_dataset = [ +# { +# "name": "sa_walk", +# "env": "hilly", +# "mode": "test", +# "nr": 0, +# "start": 200, +# "stop": 2000, +# "comment": "", +# }, +# ROOT_DIR = "/home/rschmid/RosBags/Perugia" +# perugia_dataset = [ +# { +# "name": "perugia", +# "env": "hilly", +# "mode": "test", +# "nr": 0, +# "start": 40, # Start and stop time is in seconds +# "stop": 2000, +# "comment": "", +# }, +# ROOT_DIR = "/home/rschmid/RosBags/perugia" +# perugia_dataset = [ +# { +# "name": "perugia", +# "env": "hilly", +# "mode": "test", +# "nr": 0, +# "start": 0, # Start and stop time is in seconds +# "stop": 500, +# "comment": "", +# }, +ROOT_DIR = "/home/rschmid/RosBags/6" perugia_dataset = [ { - "name": "day3/mission_data/2022-05-12T09:45:07_mission_0_day_3", + "name": "6", "env": "hilly", "mode": "test", "nr": 0, - "start": 80, - "stop": 390, - "comment": "emergency button by accident", - }, - { - "name": "day3/mission_data/2022-05-12T09:57:13_mission_0_day_3", - "env": "hilly", - "mode": "test", - "nr": 1, - "start": 50, - "stop": 430, - "comment": "fall", - }, - { - "name": "day3/mission_data/2022-05-12T10:18:16_mission_0_day_3", - "env": "hilly", - "mode": "train", - "nr": 0, - "start": 44, - "stop": 680, - "comment": "", - }, - { - "name": "day3/mission_data/2022-05-12T10:34:03_mission_0_day_3", - "env": "hilly", - "mode": "test", - "nr": 2, - "start": 42, - "stop": 450, - "comment": "", - }, - { - "name": "day3/mission_data/2022-05-12T10:45:20_mission_0_day_3", - "env": "hilly", - "mode": "test", - "nr": 3, - "start": 26, - "stop": 269, - "comment": "", - }, - { - "name": "day3/mission_data/2022-05-12T11:44:56_mission_0_day_3", - "env": "forest", - "mode": "test", - "nr": 0, - "start": 42, - "stop": 590, - "comment": "", - }, - { - "name": "day3/mission_data/2022-05-12T11:56:13_mission_0_day_3", - "env": "forest", - "mode": "train", - "nr": 0, - "start": 45, - "stop": 585, - "comment": "559 stuck with tree (traversability)", - }, - { - "name": "day3/mission_data/2022-05-12T12:08:09_mission_0_day_3", - "env": "forest", - "mode": "test", - "nr": 1, - "start": 55, - "stop": 530, - "comment": "", - }, - { - "name": "day3/mission_data/2022-05-12T15:36:30_mission_0_day_3", - "env": "grassland", - "mode": "test", - "nr": 0, - "start": 0, - "stop": 696, - "comment": "walk in the end in high flowers, and close to small trees ", - }, - { - "name": "day3/mission_data/2022-05-12T15:52:37_mission_0_day_3", - "env": "grassland", - "mode": "test", - "nr": 1, - "start": 0, - "stop": 1000, - "comment": "walking on grass and gravel road; anomoly walking close to river ", - }, - { - "name": "day3/mission_data/2022-05-12T17:36:33_mission_0_day_3", - "env": "grassland", - "mode": "train", - "nr": 0, - "start": 55, - "stop": 1160, - "comment": "features some brown rough soil; robot falls down", - }, - { - "name": "day3/mission_data/2022-05-12T18:21:23_mission_0_day_3", - "env": "grassland", - "mode": "test", - "nr": 2, - "start": 80, - "stop": 750, + "start": 0, # Start and stop time is in seconds + "stop": 500, "comment": "", }, +# ROOT_DIR = "/home/rschmid/RosBags/hoengg_bags" +# perugia_dataset = [ +# { +# "name": "hoengg", +# "env": "hilly", +# "mode": "test", +# "nr": 0, +# "start": 1000, # Start and stop time is in seconds, +120 +# "stop": 2000, +# "comment": "", +# }, + # { + # "name": "day3/mission_data/2022-05-12T09:45:07_mission_0_day_3", + # "env": "hilly", + # "mode": "test", + # "nr": 0, + # "start": 80, + # "stop": 390, + # "comment": "emergency button by accident", + # }, + # { + # "name": "day3/mission_data/2022-05-12T09:57:13_mission_0_day_3", + # "env": "hilly", + # "mode": "test", + # "nr": 1, + # "start": 50, + # "stop": 430, + # "comment": "fall", + # }, + # { + # "name": "day3/mission_data/2022-05-12T10:18:16_mission_0_day_3", + # "env": "hilly", + # "mode": "train", + # "nr": 0, + # "start": 44, + # "stop": 680, + # "comment": "", + # }, + # { + # "name": "day3/mission_data/2022-05-12T10:34:03_mission_0_day_3", + # "env": "hilly", + # "mode": "test", + # "nr": 2, + # "start": 42, + # "stop": 450, + # "comment": "", + # }, + # { + # "name": "day3/mission_data/2022-05-12T10:45:20_mission_0_day_3", + # "env": "hilly", + # "mode": "test", + # "nr": 3, + # "start": 26, + # "stop": 269, + # "comment": "", + # }, + # { + # "name": "day3/mission_data/2022-05-12T11_44_56_mission_0_day_3", + # "env": "forest", + # "mode": "test", + # "nr": 0, + # "start": 42, + # "stop": 590, + # "comment": "", + # }, + # { + # "name": "day3/mission_data/2022-05-12T11_56_13_mission_0_day_3", + # "env": "forest", + # "mode": "train", + # "nr": 0, + # "start": 45, + # "stop": 585, + # "comment": "559 stuck with tree (traversability)", + # }, + # { + # "name": "day3/mission_data/2022-05-12T12_08_09_mission_0_day_3", + # "env": "forest", + # "mode": "test", + # "nr": 1, + # "start": 55, + # "stop": 530, + # "comment": "", + # }, + # { + # "name": "day3/mission_data/2022-05-12T15:36:30_mission_0_day_3", + # "env": "grassland", + # "mode": "test", + # "nr": 0, + # "start": 0, + # "stop": 696, + # "comment": "walk in the end in high flowers, and close to small trees ", + # }, + # { + # "name": "day3/mission_data/2022-05-12T15:52:37_mission_0_day_3", + # "env": "grassland", + # "mode": "test", + # "nr": 1, + # "start": 0, + # "stop": 1000, + # "comment": "walking on grass and gravel road; anomoly walking close to river ", + # }, + # { + # "name": "day3/mission_data/2022-05-12T17:36:33_mission_0_day_3", + # "env": "grassland", + # "mode": "train", + # "nr": 0, + # "start": 55, + # "stop": 1160, + # "comment": "features some brown rough soil; robot falls down", + # }, + # { + # "name": "day3/mission_data/2022-05-12T18:21:23_mission_0_day_3", + # "env": "grassland", + # "mode": "test", + # "nr": 2, + # "start": 80, + # "stop": 750, + # "comment": "", + # }, ] diff --git a/wild_visual_navigation_ros/launch/debug/param.launch b/wild_visual_navigation_ros/launch/debug/param.launch index e69de29b..1238b45b 100644 --- a/wild_visual_navigation_ros/launch/debug/param.launch +++ b/wild_visual_navigation_ros/launch/debug/param.launch @@ -0,0 +1,7 @@ + + + + + + +