From 07981ddc785ac120eded1b866dc6705bfd2a4015 Mon Sep 17 00:00:00 2001 From: Neelaksh Singh Date: Mon, 1 Jul 2024 11:41:19 +0000 Subject: [PATCH 01/31] New launch file, possible rviz fix --- smb/launch/team6_stack.launch | 1 + smb_opc/launch/opc_rviz.launch | 5 +++++ 2 files changed, 6 insertions(+) create mode 100644 smb/launch/team6_stack.launch create mode 100644 smb_opc/launch/opc_rviz.launch diff --git a/smb/launch/team6_stack.launch b/smb/launch/team6_stack.launch new file mode 100644 index 0000000..8d889a7 --- /dev/null +++ b/smb/launch/team6_stack.launch @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/smb_opc/launch/opc_rviz.launch b/smb_opc/launch/opc_rviz.launch new file mode 100644 index 0000000..f089428 --- /dev/null +++ b/smb_opc/launch/opc_rviz.launch @@ -0,0 +1,5 @@ + + + + From 53bc5b2640b0b8e0911005e45e5e57a030d49af2 Mon Sep 17 00:00:00 2001 From: Neelaksh Singh Date: Mon, 1 Jul 2024 15:05:32 +0000 Subject: [PATCH 02/31] Adding team 6's stack --- smb/launch/team6_stack.launch | 1 - team6_stack/CMakeLists.txt | 202 ++++++++++++++++++++++++++ team6_stack/launch/team6_stack.launch | 36 +++++ team6_stack/package.xml | 59 ++++++++ 4 files changed, 297 insertions(+), 1 deletion(-) delete mode 100644 smb/launch/team6_stack.launch create mode 100644 team6_stack/CMakeLists.txt create mode 100644 team6_stack/launch/team6_stack.launch create mode 100644 team6_stack/package.xml diff --git a/smb/launch/team6_stack.launch b/smb/launch/team6_stack.launch deleted file mode 100644 index 8d889a7..0000000 --- a/smb/launch/team6_stack.launch +++ /dev/null @@ -1 +0,0 @@ - \ No newline at end of file diff --git a/team6_stack/CMakeLists.txt b/team6_stack/CMakeLists.txt new file mode 100644 index 0000000..6897b2f --- /dev/null +++ b/team6_stack/CMakeLists.txt @@ -0,0 +1,202 @@ +cmake_minimum_required(VERSION 3.0.2) +project(team6_stack) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES team6_stack +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/team6_stack.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/team6_stack_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_team6_stack.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/team6_stack/launch/team6_stack.launch b/team6_stack/launch/team6_stack.launch new file mode 100644 index 0000000..d2d49f2 --- /dev/null +++ b/team6_stack/launch/team6_stack.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/team6_stack/package.xml b/team6_stack/package.xml new file mode 100644 index 0000000..b836ea5 --- /dev/null +++ b/team6_stack/package.xml @@ -0,0 +1,59 @@ + + + team6_stack + 0.0.0 + The team6_stack package + + + + + robotx + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + + From 64ca452d10e119e148e8b5b7b6ee4fa41be4cd29 Mon Sep 17 00:00:00 2001 From: Neelaksh Singh Date: Tue, 2 Jul 2024 21:11:43 +0000 Subject: [PATCH 03/31] [Updates] Added team6_stack --- record_sensors.sh | 3 ++- smb_slam/launch/replay_SLAM.launch | 7 ++++++- team6_stack/launch/team6_stack.launch | 8 +++++--- 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/record_sensors.sh b/record_sensors.sh index 6681106..2ac060c 100755 --- a/record_sensors.sh +++ b/record_sensors.sh @@ -25,4 +25,5 @@ rosbag record --output-name=${outpath}/${now}"_smb" \ /imu \ /rslidar/points \ /tf \ -/tf_static +/tf_static \ +/clock diff --git a/smb_slam/launch/replay_SLAM.launch b/smb_slam/launch/replay_SLAM.launch index af6828d..e8e5292 100644 --- a/smb_slam/launch/replay_SLAM.launch +++ b/smb_slam/launch/replay_SLAM.launch @@ -25,9 +25,14 @@ + + - + + + + diff --git a/team6_stack/launch/team6_stack.launch b/team6_stack/launch/team6_stack.launch index d2d49f2..009e4d7 100644 --- a/team6_stack/launch/team6_stack.launch +++ b/team6_stack/launch/team6_stack.launch @@ -5,9 +5,7 @@ - - - + @@ -33,4 +31,8 @@ + + + + \ No newline at end of file From 991b023e3b92c3ecc9cb8160ae2a1777327860ec Mon Sep 17 00:00:00 2001 From: Magdalena Yordanova Date: Tue, 2 Jul 2024 22:03:09 +0000 Subject: [PATCH 04/31] add node --- team6_stack/src/team6_stack.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100755 team6_stack/src/team6_stack.py diff --git a/team6_stack/src/team6_stack.py b/team6_stack/src/team6_stack.py new file mode 100755 index 0000000..13b239e --- /dev/null +++ b/team6_stack/src/team6_stack.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python +import rospy + +from std_msgs.msg import Header +from object_detection.object_detection_msgs import ObjectDetection + +def callback(msg): + infos = [] + for info in msg.info: + info = { + 'class_id': info.class_id, + 'x': info.position.x, # in camera frame + 'y': info.position.y, # in camera frame + 'z': info.position.z # in camera frame + } + infos.append(info) + print(infos) + # TODO: Save to a csv file + +def object_detection_listener(): + rospy.init_node('team6_stack', anonymous=True) + + # Subscribe to the /object_detection/detection_info topic + rospy.Subscriber('/object_detection/detection_info', ObjectDetection, callback) + + # Keep the script running + rospy.spin() + +if __name__ == '__main__': + object_detection_listener() From 08869c25bc85110e9a1288e2169f529df92f4064 Mon Sep 17 00:00:00 2001 From: Khaled Jalloul Date: Tue, 2 Jul 2024 22:16:38 +0000 Subject: [PATCH 05/31] [MSF Graph] Added param to team6 stack, carving set to True --- smb_msf_graph/launch/smb_msf_graph.launch | 2 +- team6_stack/param/param_robosense_rs16.lua | 90 ++++++++++++++++++++++ 2 files changed, 91 insertions(+), 1 deletion(-) create mode 100644 team6_stack/param/param_robosense_rs16.lua diff --git a/smb_msf_graph/launch/smb_msf_graph.launch b/smb_msf_graph/launch/smb_msf_graph.launch index 8677771..ec13b1e 100644 --- a/smb_msf_graph/launch/smb_msf_graph.launch +++ b/smb_msf_graph/launch/smb_msf_graph.launch @@ -25,7 +25,7 @@ - + diff --git a/team6_stack/param/param_robosense_rs16.lua b/team6_stack/param/param_robosense_rs16.lua new file mode 100644 index 0000000..b4d57ea --- /dev/null +++ b/team6_stack/param/param_robosense_rs16.lua @@ -0,0 +1,90 @@ +include "default/default_parameters.lua" + + +params = deepcopy(DEFAULT_PARAMETERS) + +--ScanToScan ODOMETRY +params.odometry.scan_processing.voxel_size = 0.2 +params.odometry.scan_processing.downsampling_ratio = 1.0 +params.odometry.scan_processing.scan_cropping.cropping_radius_max = 40.0 + +--Advanced Options. +params.odometry.use_odometry_topic_instead_of_scan_to_scan = true --Uses External Odometry topic instead of Scan2Scan registration. +params.odometry.use_IMU_for_attitude_initialization = false --Uses IMU msgs to initialize gravity aligned attitude. + +--MAPPER_LOCALIZER +params.mapper_localizer.is_use_map_initialization = false +params.mapper_localizer.republish_the_preloaded_map = false +params.mapper_localizer.is_merge_scans_into_map = false +params.mapper_localizer.is_build_dense_map = false +params.mapper_localizer.is_attempt_loop_closures = false +params.mapper_localizer.is_print_timing_information = false +params.mapper_localizer.map_merge_delay_in_seconds = 10.0 +params.mapper_localizer.is_refine_odometry_constraints_between_submaps = false + +params.mapper_localizer.is_carving_enabled = true +params.mapper_localizer.scan_to_map_registration.scan_processing.voxel_size = 0.25 +params.mapper_localizer.scan_to_map_registration.scan_processing.downsampling_ratio = 1.0 +params.mapper_localizer.scan_to_map_registration.scan_processing.scan_cropping.cropping_radius_max = 15.0 --meters +params.mapper_localizer.scan_to_map_registration.icp.max_correspondence_dist = 2.0 --NOT USED RIGHT NOW +params.mapper_localizer.scan_to_map_registration.icp.knn = 5 --Currently only used for surface normal estimation. +params.mapper_localizer.scan_to_map_registration.icp.max_distance_knn = 1.0 --Currently only used for surface normal estimation. +params.mapper_localizer.scan_to_map_registration.icp.reference_cloud_seting_period = 2.0 --Seconds + +--MAP_INITIALIZER +params.map_initializer.pcd_file_package = "open3d_slam_ros" +params.map_initializer.pcd_file_path = "" --you need to give your own map here. .pcd or .ply well supported. +params.map_initializer.is_initialize_interactively = false +params.map_initializer.init_pose.x = 0.0 +params.map_initializer.init_pose.y = 2.0 +params.map_initializer.init_pose.z = 0.0 +params.map_initializer.init_pose.roll = 0.0 +params.map_initializer.init_pose.pitch = 0.0 +params.map_initializer.init_pose.yaw = 120.0 + +--SUBMAP +params.submap.submap_size = 15.0 --meters +params.submap.adjacency_based_revisiting_min_fitness = 0.5 +params.submap.min_seconds_between_feature_computation = 10.0 +params.submap.submaps_num_scan_overlap = 5 -- 200 +params.submap.max_num_points = 250000 + +--MAP_BUILDER +params.map_builder.map_voxel_size = 0.25 +params.map_builder.scan_cropping.cropping_radius_max = 40.0 +params.map_builder.scan_cropping.cropping_radius_min = 2.0 +params.map_builder.space_carving.carve_space_every_n_scans = 10 +params.map_builder.space_carving.max_raytracing_length = 20 +params.map_builder.space_carving.truncation_distance = 0.3 +params.map_builder.space_carving.voxel_size = 0.2 + +--DENSE_MAP_BUILDER +params.dense_map_builder.map_voxel_size = 0.05 +params.dense_map_builder.scan_cropping.cropping_radius_max = 105.0 +params.dense_map_builder.space_carving.carve_space_every_n_scans = 10 +params.dense_map_builder.space_carving.truncation_distance = 0.1 + +--PLACE_RECOGNITION +params.place_recognition.ransac_min_corresondence_set_size = 40 +params.place_recognition.max_icp_correspondence_distance = 1.0 +params.place_recognition.min_icp_refinement_fitness = 0.7 +params.place_recognition.dump_aligned_place_recognitions_to_file = false +params.place_recognition.min_submaps_between_loop_closures = 2 +params.place_recognition.loop_closure_search_radius = 20.0 --This considers a looot of submaps. With the current setup. +params.place_recognition.consistency_check.max_drift_roll = 30.0 --deg +params.place_recognition.consistency_check.max_drift_pitch = 30.0 --deg +params.place_recognition.consistency_check.max_drift_yaw = 30.0 --deg +params.place_recognition.consistency_check.max_drift_x = 80.0 --m +params.place_recognition.consistency_check.max_drift_y = 80.0 --m +params.place_recognition.consistency_check.max_drift_z = 40.0 --m + +--MOTION_COMPENSATION +params.motion_compensation.is_undistort_scan = false +params.motion_compensation.num_poses_vel_estimation = 3 + +--SAVING +params.saving.save_map = true +params.saving.save_submaps = false + + +return params \ No newline at end of file From 37e137d849290614f9e7c39891d6b5498262252c Mon Sep 17 00:00:00 2001 From: Khaled Jalloul Date: Tue, 2 Jul 2024 22:44:51 +0000 Subject: [PATCH 06/31] [Space Carving] Added desc, added default param, voxel size test to follow --- smb_msf_graph/launch/smb_msf_graph.launch | 2 +- .../msf_graph/default/default_parameters.lua | 16 ++ .../parameter_structure_definitions.lua | 182 ++++++++++++++++++ .../{ => msf_graph}/param_robosense_rs16.lua | 8 +- 4 files changed, 203 insertions(+), 5 deletions(-) create mode 100644 team6_stack/param/msf_graph/default/default_parameters.lua create mode 100644 team6_stack/param/msf_graph/default/parameter_structure_definitions.lua rename team6_stack/param/{ => msf_graph}/param_robosense_rs16.lua (91%) diff --git a/smb_msf_graph/launch/smb_msf_graph.launch b/smb_msf_graph/launch/smb_msf_graph.launch index ec13b1e..76c8f24 100644 --- a/smb_msf_graph/launch/smb_msf_graph.launch +++ b/smb_msf_graph/launch/smb_msf_graph.launch @@ -25,7 +25,7 @@ - + diff --git a/team6_stack/param/msf_graph/default/default_parameters.lua b/team6_stack/param/msf_graph/default/default_parameters.lua new file mode 100644 index 0000000..07a9615 --- /dev/null +++ b/team6_stack/param/msf_graph/default/default_parameters.lua @@ -0,0 +1,16 @@ +include "parameter_structure_definitions.lua" + + +DEFAULT_PARAMETERS = { + odometry = deepcopy(ODOMETRY_PARAMETERS), + submap = deepcopy(SUBMAP_PARAMETERS), + map_builder = deepcopy(MAP_BUILDER_PARAMETERS), + dense_map_builder = deepcopy(MAP_BUILDER_PARAMETERS), + mapper_localizer = deepcopy(MAPPER_LOCALIZER_PARAMETERS), + saving = deepcopy(SAVING_PARAMETERS), + visualization = deepcopy(VISUALIZATION_PARAMETERS), + motion_compensation = deepcopy(MOTION_COMPENSATION_PARAMETERS), + global_optimization = deepcopy(GLOBAL_OPTIMIZATION_PARAMETERS), + map_initializer = deepcopy(MAP_INITIALIZER_PARAMETERS), + place_recognition = deepcopy(PLACE_RECOGNITION_PARAMETERS), +} \ No newline at end of file diff --git a/team6_stack/param/msf_graph/default/parameter_structure_definitions.lua b/team6_stack/param/msf_graph/default/parameter_structure_definitions.lua new file mode 100644 index 0000000..ec6bee5 --- /dev/null +++ b/team6_stack/param/msf_graph/default/parameter_structure_definitions.lua @@ -0,0 +1,182 @@ +-- need this to deep copy all the tables +function deepcopy(orig, copies) + copies = copies or {} + local orig_type = type(orig) + local copy + if orig_type == 'table' then + if copies[orig] then + copy = copies[orig] + else + copy = {} + copies[orig] = copy + for orig_key, orig_value in next, orig, nil do + copy[deepcopy(orig_key, copies)] = deepcopy(orig_value, copies) + end + setmetatable(copy, deepcopy(getmetatable(orig), copies)) + end + else -- number, string, boolean, etc + copy = orig + end + return copy +end + + + +SAVING_PARAMETERS = { + save_at_mission_end = true, + save_map = true, + save_submaps = false, + save_dense_submaps = true, +} + +MOTION_COMPENSATION_PARAMETERS = { + is_undistort_scan = false, + is_spinning_clockwise = true, + scan_duration = 0.1, + num_poses_vel_estimation = 3, +} + +VISUALIZATION_PARAMETERS = { + assembled_map_voxel_size = 0.05, + submaps_voxel_size = 0.05, + visualize_every_n_msec = 500.0, +} + +GLOBAL_OPTIMIZATION_PARAMETERS = { + edge_prune_threshold = 0.2, + loop_closure_preference = 2.0, + max_correspondence_distance = 1000.0, + reference_node = 0, +} + +SCAN_CROPPING_PARAMETERS = { + cropping_radius_max= 100.0, + cropping_radius_min= 2.0, + min_z= -50.0, + max_z= 50.0, + cropper_type= "MinMaxRadius", -- options are Cylinder, MaxRadius, MinRadius, MinMaxRadius +} + +SCAN_PROCESSING_PARAMETERS = { + voxel_size = 0.01, + downsampling_ratio = 1.0, + point_cloud_buffer_size = 1, -- the scan processing buffer size. 1 means no buffering. + scan_cropping = deepcopy(SCAN_CROPPING_PARAMETERS), +} + +ICP_PARAMETERS = { + max_correspondence_dist= 1.0, + knn= 10, + max_distance_knn= 1.0, + max_n_iter= 30, + reference_cloud_seting_period= 1.0, +} + +SCAN_MATCHING_PARAMETERS = { + icp = deepcopy(ICP_PARAMETERS), + cloud_registration_type = "GeneralizedIcp", -- options GeneralizedIcp, PointToPointIcp, PointToPlaneIcp +} + +ODOMETRY_PARAMETERS = { + use_odometry_topic_instead_of_scan_to_scan = true, + use_IMU_for_attitude_initialization = false, + is_publish_odometry_msgs = true, + odometry_buffer_size = 1, -- the scan2scan odometry buffer size. 1 means no buffering. + scan_matching = deepcopy(SCAN_MATCHING_PARAMETERS), + scan_processing = deepcopy(SCAN_PROCESSING_PARAMETERS), +} + +SUBMAP_PARAMETERS = { + submap_size = 20, -- meters + min_num_range_data = 10, + adjacency_based_revisiting_min_fitness = 0.5, + min_seconds_between_feature_computation = 5.0, + submaps_num_scan_overlap = 10, +} + +SPACE_CARVING_PARAMETERS = { + voxel_size= 0.2, + max_raytracing_length = 20.0, + truncation_distance = 0.3, + carve_space_every_n_scans= 10.0, + min_dot_product_with_normal = 0.5, + neigborhood_radius_for_removal= 0.1, +} + +MAP_BUILDER_PARAMETERS = { + map_voxel_size = 0.05, --meters + scan_cropping = deepcopy(SCAN_CROPPING_PARAMETERS), + space_carving = deepcopy(SPACE_CARVING_PARAMETERS), +} + +SCAN_TO_MAP_REGISTRATION_PARAMETERS = { + min_refinement_fitness = 0.7, + scan_to_map_refinement_type = "GeneralizedIcp", -- options GeneralizedIcp, PointToPointIcp, PointToPlaneIcp + icp = deepcopy(ICP_PARAMETERS), + scan_processing = deepcopy(SCAN_PROCESSING_PARAMETERS), +} + +MAPPER_LOCALIZER_PARAMETERS = { + is_print_timing_information = true, + is_carving_enabled = false, + is_build_dense_map = false, + is_attempt_loop_closures = false, + is_use_map_initialization = false, + republish_the_preloaded_map = true, + map_merge_delay_in_seconds = 10.0, + is_merge_scans_into_map = false, + dump_submaps_to_file_before_after_lc = false, + is_refine_odometry_constraints_between_submaps = false, + min_movement_between_mapping_steps = 0.0, + ignore_minimum_refinement_fitness = false, + mapping_buffer_size = 1, -- the scan2map odometry buffer size. 1 means no buffering. + scan_to_map_registration = deepcopy(SCAN_TO_MAP_REGISTRATION_PARAMETERS), +} + +POSE = { + x = 0.0, + y = 0.0, + z = 0.0, + roll = 0.0, --roll, pitch ,yaw in degrees!!!!! + pitch = 0.0, + yaw = 0.0, +} + +MAP_INITIALIZER_PARAMETERS = { + is_initialize_interactively = false, + frame_id = "map_o3d", + pcd_file_path = "", + pcd_file_package = "", + init_pose = POSE, +} + +LOOP_CLOSURE_CONSISTENCY_CHECK_PARAMETERS = { + max_drift_roll = 30.0, --deg + max_drift_pitch = 30.0, --deg + max_drift_yaw = 30.0, --deg + max_drift_x = 80.0, --meters + max_drift_y = 80.0, --meters + max_drift_z = 40.0, --meters +} + +PLACE_RECOGNITION_PARAMETERS = { + feature_map_normal_estimation_radius = 2.0, + feature_voxel_size = 0.5, + feature_radius = 2.5, + feature_knn = 100, + feature_normal_knn = 20, + ransac_num_iter = 10000000, + ransac_probability = 0.999, + ransac_model_size = 3, + ransac_max_correspondence_dist = 0.75, + ransac_correspondence_checker_distance = 0.8, + ransac_correspondence_checker_edge_length = 0.6, + ransac_min_corresondence_set_size = 25, + max_icp_correspondence_distance = 0.3, + min_icp_refinement_fitness = 0.7, -- the more aliasing, the higher this should be + dump_aligned_place_recognitions_to_file = false , --useful for debugging + min_submaps_between_loop_closures = 2, + loop_closure_search_radius = 20.0, + consistency_check = deepcopy(LOOP_CLOSURE_CONSISTENCY_CHECK_PARAMETERS), +} + diff --git a/team6_stack/param/param_robosense_rs16.lua b/team6_stack/param/msf_graph/param_robosense_rs16.lua similarity index 91% rename from team6_stack/param/param_robosense_rs16.lua rename to team6_stack/param/msf_graph/param_robosense_rs16.lua index b4d57ea..2e6c6ce 100644 --- a/team6_stack/param/param_robosense_rs16.lua +++ b/team6_stack/param/msf_graph/param_robosense_rs16.lua @@ -53,10 +53,10 @@ params.submap.max_num_points = 250000 params.map_builder.map_voxel_size = 0.25 params.map_builder.scan_cropping.cropping_radius_max = 40.0 params.map_builder.scan_cropping.cropping_radius_min = 2.0 -params.map_builder.space_carving.carve_space_every_n_scans = 10 -params.map_builder.space_carving.max_raytracing_length = 20 -params.map_builder.space_carving.truncation_distance = 0.3 -params.map_builder.space_carving.voxel_size = 0.2 +params.map_builder.space_carving.carve_space_every_n_scans = 10 --decreasing will increase commputation load +params.map_builder.space_carving.max_raytracing_length = 20 --self explanatory +params.map_builder.space_carving.truncation_distance = 0.3 --roughly the min ray tracing distance for carving +params.map_builder.space_carving.voxel_size = 0.2 --Bigger value results in more aggresive pruning --DENSE_MAP_BUILDER params.dense_map_builder.map_voxel_size = 0.05 From 197d215731e481be162b3ff6d6ba6469e4cb98c5 Mon Sep 17 00:00:00 2001 From: YifanZhou99 <75839011+YifanZhou99@users.noreply.github.com> Date: Wed, 3 Jul 2024 07:36:42 +0000 Subject: [PATCH 07/31] add tf_listerner, not tested --- team6_stack/src/tf_listener.py | 78 ++++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100644 team6_stack/src/tf_listener.py diff --git a/team6_stack/src/tf_listener.py b/team6_stack/src/tf_listener.py new file mode 100644 index 0000000..ce23ec2 --- /dev/null +++ b/team6_stack/src/tf_listener.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python + +import rospy +import tf2_ros +import tf2_msgs +from geometry_msgs.msg import TransformStamped +import tf2_geometry_msgs + +from tf2_msgs.msg import TFMessage +from custom_msgs.msg import DetectionInfo # Adjust this import based on the actual message type +import numpy as np + +tf_buffer = None +tf_listener = None + +def tf_static_callback(msg): + for transform in msg.transforms: + # base_link -> rgb_camera_link + if transform.header.frame_id == "base_link" and transform.child_frame_id == "rgb_camera_link": + translation_base2cam = transform.transform.translation + rotation_base2cam = transform.transform.rotation + rospy.loginfo(f"Translation_base2cam: x={translation_base2cam.x}, y={translation_base2cam.y}, z={translation_base2cam.z}") + rospy.loginfo(f"Rotation_base2cam: x={rotation_base2cam.x}, y={rotation_base2cam.y}, z={rotation_base2cam.z}, w={rotation_base2cam.w}") + # rgb_camera_link -> rgb_camera_optical_link + if transform.header.frame_id == "rgb_camera_link" and transform.child_frame_id == "rgb_camera_optical_link": + translation_cam2opt = transform.transform.translation + rotation_cam2opt = transform.transform.rotation + rospy.loginfo(f"Translation_cam2opt: x={translation_cam2opt.x}, y={translation_cam2opt.y}, z={translation_cam2opt.z}") + rospy.loginfo(f"Rotation_cam2opt: x={rotation_cam2opt.x}, y={rotation_cam2opt.y}, z={rotation_cam2opt.z}, w={rotation_cam2opt.w}") + +def tf_callback(msg): + for transform in msg.transforms: + if transform.header.frame_id == "base_link" and transform.child_frame_id == "rgb_camera_link": + translation = transform.transform.translation + rotation = transform.transform.rotation + #rospy.loginfo(f"Translation: x={translation.x}, y={translation.y}, z={translation.z}") + #rospy.loginfo(f"Rotation: x={rotation.x}, y={rotation.y}, z={rotation.z}, w={rotation.w}") + +def detection_callback(msg): + for detection in msg.info: + if detection.frame_id == "rgb_camera_optical_link": + object_position = detection.position + class_id = detection.class_id + + # Create a PoseStamped message for the detected object's position + object_pose = tf2_geometry_msgs.PoseStamped() + object_pose.header.frame_id = "rgb_camera_optical_link" + object_pose.pose.position = object_position + object_pose.pose.orientation.w = 1.0 + + try: + # Transform the object position to the base_link frame + transformed_pose = tf_buffer.transform(object_pose, "base_link", rospy.Duration(1.0)) + + transformed_position = transformed_pose.pose.position + rospy.loginfo(f"Detected object (class_id: {class_id}) position in base_link frame: x={transformed_position.x}, y={transformed_position.y}, z={transformed_position.z}") + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + rospy.logerr(f"TF transform error: {e}") + +def duplicate_rejection(): + #TODO: if the global postions of multiple detected objects are too close. Keep one and reject others. + +def main(): + global tf_buffer, tf_listener + + rospy.init_node('tf_listener', anonymous=True) + + tf_buffer = tf2_ros.Buffer() + tf_listener = tf2_ros.TransformListener(tf_buffer) + + rospy.Subscriber('/tf_static', tf2_msgs.msg.TFMessage, tf_static_callback) + rospy.Subscriber('/tf_static', TFMessage, tf_callback) + rospy.Subscriber('/object_detector/detection_info', DetectionInfo, detection_callback) + + rospy.spin() + +if __name__ == '__main__': + main() From f6864b07247fa5513b30dac1b7cdf69a6b5629a4 Mon Sep 17 00:00:00 2001 From: YifanZhou99 <75839011+YifanZhou99@users.noreply.github.com> Date: Wed, 3 Jul 2024 07:41:54 +0000 Subject: [PATCH 08/31] change src to scripts --- smb_opc/rviz/smb_vis.rviz | 100 +++++++------------- team6_stack/{src => scripts}/team6_stack.py | 0 team6_stack/{src => scripts}/tf_listener.py | 0 3 files changed, 32 insertions(+), 68 deletions(-) rename team6_stack/{src => scripts}/team6_stack.py (100%) rename team6_stack/{src => scripts}/tf_listener.py (100%) diff --git a/smb_opc/rviz/smb_vis.rviz b/smb_opc/rviz/smb_vis.rviz index df59ac6..de7b146 100644 --- a/smb_opc/rviz/smb_vis.rviz +++ b/smb_opc/rviz/smb_vis.rviz @@ -16,7 +16,7 @@ Panels: - /FarPlanner1/GlobalGraph1 - /TarePlanner1 Splitter Ratio: 0.5111420750617981 - Tree Height: 770 + Tree Height: 498 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -85,8 +85,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.5905910730361938 - Min Value: -1.1293858289718628 + Max Value: 2.262173652648926 + Min Value: -0.20957785844802856 Value: true Axis: Z Channel Name: intensity @@ -133,25 +133,13 @@ Visualization Manager: Value: true base_link: Value: true - camera: - Value: true imu: Value: true imu_link: Value: true lidar_mount_link: Value: true - map_o3d: - Value: true - map_o3d_graph_msf_aligned: - Value: true - map_o3d_viz: - Value: true - odom_graph_msf: - Value: true - odom_o3d: - Value: true - raw_rs_o3d: + odom: Value: true rgb_camera_link: Value: true @@ -161,16 +149,8 @@ Visualization Manager: Value: true rslidar_base_link: Value: true - sensor: - Value: true - sensor_at_scan: - Value: true top: Value: true - vehicle: - Value: true - world_graph_msf: - Value: true Marker Alpha: 1 Marker Scale: 1 Name: TF @@ -178,46 +158,30 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - base_link: - LF_WHEEL: - {} - LH_WHEEL: - {} - RF_WHEEL: - {} - RH_WHEEL: - {} - base_inertia: - {} - imu_link: - imu: + odom: + base_link: + LF_WHEEL: {} - lidar_mount_link: - rslidar_base_link: - rslidar: - map_o3d: - map_o3d_viz: - {} - raw_rs_o3d: - {} - odom_o3d: - {} - sensor: - camera: - {} - vehicle: - {} - odom_graph_msf: - world_graph_msf: - map_o3d_graph_msf_aligned: + LH_WHEEL: + {} + RF_WHEEL: + {} + RH_WHEEL: + {} + base_inertia: + {} + imu_link: + imu: {} - sensor_at_scan: + lidar_mount_link: + rslidar_base_link: + rslidar: + {} + rgb_camera_link: + rgb_camera_optical_link: {} - rgb_camera_link: - rgb_camera_optical_link: + top: {} - top: - {} Update Interval: 0 Value: true - Alpha: 1 @@ -926,7 +890,7 @@ Visualization Manager: Marker Topic: /sensor_coverage_planner/tare_visualizer/local_planning_horizon Name: LocalPlanningHorizon Namespaces: - "": true + {} Queue Size: 100 Value: true - Class: rviz/Marker @@ -934,7 +898,7 @@ Visualization Manager: Marker Topic: /sensor_coverage_planner/tare_visualizer/exploring_subspaces Name: ExplorationSubspaces Namespaces: - "": true + {} Queue Size: 100 Value: true - Alpha: 1 @@ -1067,21 +1031,21 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7397965788841248 + Pitch: 0.7347965836524963 Target Frame: - Yaw: 3.385552406311035 + Yaw: 3.3455517292022705 Saved: ~ Window Geometry: ControlPanel: collapsed: false Displays: collapsed: false - Height: 1536 + Height: 1136 Hide Left Dock: false Hide Right Dock: true Image bounding Boxes: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000002d000000542fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d000003a2000000fb0100001cfa000000010100000002fb000000180043006f006e00740072006f006c00500061006e0065006c0100000000ffffffff000000d200fffffffb000000100044006900730070006c0061007900730100000000000001ea0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000003e5000000b70000007300fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f0061006401000004a2000000dd0000007300ffffff00000001000001c3000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a400fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000001600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070f0000005efc0100000002fb0000000800540069006d006501000000000000070f000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004390000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002d0000003b6fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003b0000028e000000ed0100001afa000000010100000002fb000000180043006f006e00740072006f006c00500061006e0065006c0100000000ffffffff000000bf00fffffffb000000100044006900730070006c0061007900730100000000000001ea0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000002cf000000800000006d00fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f0061006401000003550000009c0000006d00ffffff00000001000001c3000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a000fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000001600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000462000003b600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Raw Image: collapsed: false SMBPowerMotor: @@ -1096,6 +1060,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1807 - X: 0 + Width: 1848 + X: 72 Y: 27 diff --git a/team6_stack/src/team6_stack.py b/team6_stack/scripts/team6_stack.py similarity index 100% rename from team6_stack/src/team6_stack.py rename to team6_stack/scripts/team6_stack.py diff --git a/team6_stack/src/tf_listener.py b/team6_stack/scripts/tf_listener.py similarity index 100% rename from team6_stack/src/tf_listener.py rename to team6_stack/scripts/tf_listener.py From 9bfbb0c71897647cec63020ee47063235b975b25 Mon Sep 17 00:00:00 2001 From: Magdalena Yordanova Date: Wed, 3 Jul 2024 07:50:19 +0000 Subject: [PATCH 09/31] writing to a csv file, not tested --- team6_stack/scripts/team6_stack.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/team6_stack/scripts/team6_stack.py b/team6_stack/scripts/team6_stack.py index 13b239e..07880fb 100755 --- a/team6_stack/scripts/team6_stack.py +++ b/team6_stack/scripts/team6_stack.py @@ -2,7 +2,9 @@ import rospy from std_msgs.msg import Header -from object_detection.object_detection_msgs import ObjectDetection +from object_detection_msgs import ObjectDetectionInfoArray + +# TODO: needs to be tested def callback(msg): infos = [] @@ -14,14 +16,20 @@ def callback(msg): 'z': info.position.z # in camera frame } infos.append(info) + print(infos) - # TODO: Save to a csv file + + # Save the detected artifacts to a csv file + with open('data/artifacts.csv', mode='a', newline='') as file: + writer = csv.writer(file) + for info in infos: + writer.writerow([info['class_id'], info['x'], info['y'], info['z']]) def object_detection_listener(): rospy.init_node('team6_stack', anonymous=True) - # Subscribe to the /object_detection/detection_info topic - rospy.Subscriber('/object_detection/detection_info', ObjectDetection, callback) + # Subscribe to the /object_detector/detection_info topic + rospy.Subscriber('/object_detector/detection_info', ObjectDetectionInfoArray, callback) # Keep the script running rospy.spin() From 116595c5c5f7ed85841dc8bfba2b51a6aeddcb9c Mon Sep 17 00:00:00 2001 From: Magdalena Yordanova Date: Wed, 3 Jul 2024 08:19:54 +0000 Subject: [PATCH 10/31] adjusting the node to accepting also py files --- team6_stack/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/team6_stack/CMakeLists.txt b/team6_stack/CMakeLists.txt index 6897b2f..efa9983 100644 --- a/team6_stack/CMakeLists.txt +++ b/team6_stack/CMakeLists.txt @@ -155,10 +155,10 @@ include_directories( ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +catkin_install_python(PROGRAMS + scripts/team6_stack.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html From ec7d6e56d2abae42f7375b48bf164050fa439dc4 Mon Sep 17 00:00:00 2001 From: YifanZhou99 <75839011+YifanZhou99@users.noreply.github.com> Date: Wed, 3 Jul 2024 08:26:48 +0000 Subject: [PATCH 11/31] tf_listener completed not tested --- frames.gv | 25 ++++++++++ team6_stack/scripts/tf_listener.py | 74 +++++++++++++++++++++++------- 2 files changed, 83 insertions(+), 16 deletions(-) create mode 100644 frames.gv diff --git a/frames.gv b/frames.gv new file mode 100644 index 0000000..c3e715b --- /dev/null +++ b/frames.gv @@ -0,0 +1,25 @@ +digraph G { +"base_link" -> "base_inertia"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"]; +"odom" -> "base_link"[label="Broadcaster: /ekf_se\nAverage rate: 30.612 Hz\nMost recent transform: 2862.872 ( 0.038 sec old)\nBuffer length: 1.960 sec\n"]; +"imu_link" -> "imu"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"]; +"base_link" -> "imu_link"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"]; +"base_link" -> "lidar_mount_link"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"]; +"base_link" -> "rgb_camera_link"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"]; +"rgb_camera_link" -> "rgb_camera_optical_link"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"]; +"lidar_mount_link" -> "rslidar_base_link"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"]; +"rslidar_base_link" -> "rslidar"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"]; +"base_link" -> "top"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"]; +"odom_graph_msf" -> "world_graph_msf"[label="Broadcaster: /smb_estimator_node\nAverage rate: 386.500 Hz\nMost recent transform: 2862.902 ( 0.008 sec old)\nBuffer length: 2.000 sec\n"]; +"base_link" -> "odom_graph_msf"[label="Broadcaster: /smb_estimator_node\nAverage rate: 386.114 Hz\nMost recent transform: 2862.905 ( 0.005 sec old)\nBuffer length: 2.002 sec\n"]; +"base_link" -> "LF_WHEEL"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 50.505 Hz\nMost recent transform: 2862.891 ( 0.019 sec old)\nBuffer length: 1.980 sec\n"]; +"base_link" -> "LH_WHEEL"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 50.505 Hz\nMost recent transform: 2862.891 ( 0.019 sec old)\nBuffer length: 1.980 sec\n"]; +"base_link" -> "RF_WHEEL"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 50.505 Hz\nMost recent transform: 2862.891 ( 0.019 sec old)\nBuffer length: 1.980 sec\n"]; +"base_link" -> "RH_WHEEL"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 50.505 Hz\nMost recent transform: 2862.891 ( 0.019 sec old)\nBuffer length: 1.980 sec\n"]; +"world_graph_msf" -> "map_o3d_graph_msf_aligned"[label="Broadcaster: /smb_estimator_node\nAverage rate: 13.193 Hz\nMost recent transform: 2862.750 ( 0.160 sec old)\nBuffer length: 1.895 sec\n"]; +"map_o3d" -> "raw_rs_o3d"[label="Broadcaster: /mapping\nAverage rate: 10.582 Hz\nMost recent transform: 2862.855 ( 0.055 sec old)\nBuffer length: 1.890 sec\n"]; +"rslidar" -> "map_o3d"[label="Broadcaster: /mapping\nAverage rate: 10.582 Hz\nMost recent transform: 2862.855 ( 0.055 sec old)\nBuffer length: 1.890 sec\n"]; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 2862.910"[ shape=plaintext ] ; + }->"odom"; +} \ No newline at end of file diff --git a/team6_stack/scripts/tf_listener.py b/team6_stack/scripts/tf_listener.py index ce23ec2..e246a97 100644 --- a/team6_stack/scripts/tf_listener.py +++ b/team6_stack/scripts/tf_listener.py @@ -3,17 +3,53 @@ import rospy import tf2_ros import tf2_msgs +from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped import tf2_geometry_msgs +from object_detection.object_detection_msgs import ObjectDetectionInfoArray -from tf2_msgs.msg import TFMessage -from custom_msgs.msg import DetectionInfo # Adjust this import based on the actual message type import numpy as np tf_buffer = None tf_listener = None +translation_odomGraph2worldGraph = None +rotation_odomGraph2worldGraph = None +translation_base2odomGraph = None +rotation_base2odomGraph = None +translation_world2base = None +rotation_world2base = None +translation_base2cam = None +rotation_base2cam = None +translation_cam2opt = None +rotation_cam2opt = None + +def tf_callback(msg): + + global translation_odomGraph2worldGraph, rotation_odomGraph2worldGraph + global translation_base2odomGraph, rotation_base2odomGraph + global translation_world2base, rotation_world2base + + for transform in msg.transforms: + # world -> base_link + if transform.header.frame_id == "odom_graph_msf" and transform.child_frame_id == "world_graph_msf" + translation_odomGraph2worldGraph = transform.transform.translation + rotation_odomGraph2worldGraph = transform.transform.rotation + if transfrom.header.frame_id == "base_link" and transform.child_frame_id == "odom_graph_msf": + translation_base2odomGraph = transform.transform.translation + rotation_base2odomGraph = transform.transform.rotation + + if transform.header.frame_id == "odom" and transform.child_frame_id == "base_link": + translation_world2base = transform.transform.translation + rotation_world2base = transform.transform.rotation + rospy.loginfo(f"Translation_world2base: x={translation_world2base.x}, y={translation_world2base.y}, z={translation_world2base.z}") + rospy.loginfo(f"Rotation_world2base: x={rotation_world2base.x}, y={rotation_world2base.y}, z={rotation_world2base.z}, w={rotation_world2base.w}") + + def tf_static_callback(msg): + global translation_base2cam, rotation_base2cam + global translation_cam2opt, rotation_cam2opt + for transform in msg.transforms: # base_link -> rgb_camera_link if transform.header.frame_id == "base_link" and transform.child_frame_id == "rgb_camera_link": @@ -28,13 +64,6 @@ def tf_static_callback(msg): rospy.loginfo(f"Translation_cam2opt: x={translation_cam2opt.x}, y={translation_cam2opt.y}, z={translation_cam2opt.z}") rospy.loginfo(f"Rotation_cam2opt: x={rotation_cam2opt.x}, y={rotation_cam2opt.y}, z={rotation_cam2opt.z}, w={rotation_cam2opt.w}") -def tf_callback(msg): - for transform in msg.transforms: - if transform.header.frame_id == "base_link" and transform.child_frame_id == "rgb_camera_link": - translation = transform.transform.translation - rotation = transform.transform.rotation - #rospy.loginfo(f"Translation: x={translation.x}, y={translation.y}, z={translation.z}") - #rospy.loginfo(f"Rotation: x={rotation.x}, y={rotation.y}, z={rotation.z}, w={rotation.w}") def detection_callback(msg): for detection in msg.info: @@ -46,19 +75,32 @@ def detection_callback(msg): object_pose = tf2_geometry_msgs.PoseStamped() object_pose.header.frame_id = "rgb_camera_optical_link" object_pose.pose.position = object_position - object_pose.pose.orientation.w = 1.0 + object_pose.pose.orientation.w = 1.0 # Don't care about the orientation try: # Transform the object position to the base_link frame transformed_pose = tf_buffer.transform(object_pose, "base_link", rospy.Duration(1.0)) - transformed_position = transformed_pose.pose.position rospy.loginfo(f"Detected object (class_id: {class_id}) position in base_link frame: x={transformed_position.x}, y={transformed_position.y}, z={transformed_position.z}") except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logerr(f"TF transform error: {e}") -def duplicate_rejection(): - #TODO: if the global postions of multiple detected objects are too close. Keep one and reject others. +def duplicate_rejection(detections): + unique_detections = [] + for i, detection in enumerate(detections): + is_duplicate = False + for unique_detection in unique_detections: + dist = np.sqrt( + (detection.position.x - unique_detection.position.x) ** 2 + + (detection.position.y - unique_detection.position.y) ** 2 + + (detection.position.z - unique_detection.position.z) ** 2 + ) + if dist < 0.1: # If objects are closer than 10 cm, consider them duplicates + is_duplicate = True + break + if not is_duplicate: + unique_detections.append(detection) + return unique_detections def main(): global tf_buffer, tf_listener @@ -68,9 +110,9 @@ def main(): tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) - rospy.Subscriber('/tf_static', tf2_msgs.msg.TFMessage, tf_static_callback) - rospy.Subscriber('/tf_static', TFMessage, tf_callback) - rospy.Subscriber('/object_detector/detection_info', DetectionInfo, detection_callback) + rospy.Subscriber('/tf', TFMessage, tf_callback) + rospy.Subscriber('/tf_static', TFMessage, tf_static_callback) + rospy.Subscriber('/object_detector/detection_info', ObjectDetectionInfoArray, detection_callback) rospy.spin() From 8b3ffb641313d751f0d1e436eb5f2b7281f9e910 Mon Sep 17 00:00:00 2001 From: Magdalena Yordanova Date: Wed, 3 Jul 2024 08:41:51 +0000 Subject: [PATCH 12/31] adjusting CMakeLists to get object_detection_msgs --- team6_stack/CMakeLists.txt | 20 ++++++++++++++------ team6_stack/scripts/team6_stack.py | 2 +- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/team6_stack/CMakeLists.txt b/team6_stack/CMakeLists.txt index efa9983..cda589b 100644 --- a/team6_stack/CMakeLists.txt +++ b/team6_stack/CMakeLists.txt @@ -7,7 +7,9 @@ project(team6_stack) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED) +find_package(catkin REQUIRED + object_detection + ) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -43,11 +45,17 @@ find_package(catkin REQUIRED) ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) +add_message_files( + DIRECTORY msg + FILES + ObjectDetection.msg + ObjectDetectionArray.msg + ObjectPosition.msg + ObjectPositionArray.msg + ObjectDetectionInfo.msg + ObjectDetectionInfoArray.msg + PointCloudArray.msg +) ## Generate services in the 'srv' folder # add_service_files( diff --git a/team6_stack/scripts/team6_stack.py b/team6_stack/scripts/team6_stack.py index 07880fb..aae2e3e 100755 --- a/team6_stack/scripts/team6_stack.py +++ b/team6_stack/scripts/team6_stack.py @@ -2,7 +2,7 @@ import rospy from std_msgs.msg import Header -from object_detection_msgs import ObjectDetectionInfoArray +from object_detection_msgs.msg import ObjectDetectionInfoArray # TODO: needs to be tested From fe14c43614ba8b9ec91cb9ffd66e665406a1d657 Mon Sep 17 00:00:00 2001 From: smb_team6 Date: Wed, 3 Jul 2024 10:45:53 +0200 Subject: [PATCH 13/31] record_sensors.sh --- record_sensors.sh | 3 +- .../param_robosense_rs16_team6.lua | 90 +++++++++++++++++++ smb_slam/launch/online_slam.launch | 2 +- 3 files changed, 93 insertions(+), 2 deletions(-) create mode 100644 smb_slam/config/open3d_slam/param_robosense_rs16_team6.lua diff --git a/record_sensors.sh b/record_sensors.sh index 2ac060c..c015a91 100755 --- a/record_sensors.sh +++ b/record_sensors.sh @@ -26,4 +26,5 @@ rosbag record --output-name=${outpath}/${now}"_smb" \ /rslidar/points \ /tf \ /tf_static \ -/clock +/clock \ +/object_detector/detection_info diff --git a/smb_slam/config/open3d_slam/param_robosense_rs16_team6.lua b/smb_slam/config/open3d_slam/param_robosense_rs16_team6.lua new file mode 100644 index 0000000..4f08013 --- /dev/null +++ b/smb_slam/config/open3d_slam/param_robosense_rs16_team6.lua @@ -0,0 +1,90 @@ +include "default/default_parameters.lua" + + +params = deepcopy(DEFAULT_PARAMETERS) + +--ScanToScan ODOMETRY +params.odometry.scan_processing.voxel_size = 0.2 +params.odometry.scan_processing.downsampling_ratio = 1.0 +params.odometry.scan_processing.scan_cropping.cropping_radius_max = 40.0 + +--Advanced Options. +params.odometry.use_odometry_topic_instead_of_scan_to_scan = true --Uses External Odometry topic instead of Scan2Scan registration. +params.odometry.use_IMU_for_attitude_initialization = false --Uses IMU msgs to initialize gravity aligned attitude. + +--MAPPER_LOCALIZER +params.mapper_localizer.is_use_map_initialization = false --If enabled, a map should be given by the params below. +params.mapper_localizer.republish_the_preloaded_map = false --if enabled, the pre-loaded map is re-published for viz. Costly. +params.mapper_localizer.is_merge_scans_into_map = false --if enabled, the provided map is extended with the scans we see. +params.mapper_localizer.is_build_dense_map = false --if enabled a dense map is built alongside the regular map. If you want this to be saved, enable submap saving. +params.mapper_localizer.is_attempt_loop_closures = false +params.mapper_localizer.is_print_timing_information = false --if enabled additional logs are printed +params.mapper_localizer.map_merge_delay_in_seconds = 10.0 --the delay in seconds before merging to the existing map. +params.mapper_localizer.is_refine_odometry_constraints_between_submaps = false + +params.mapper_localizer.is_carving_enabled = true --Space carving, allows user to clear the dynamic obstables from the map if points are re-observed. +params.mapper_localizer.scan_to_map_registration.scan_processing.voxel_size = 0.25 +params.mapper_localizer.scan_to_map_registration.scan_processing.downsampling_ratio = 1.0 -- 1.0: no down sampling, 0.1: 1 in every 10 points left. +params.mapper_localizer.scan_to_map_registration.scan_processing.scan_cropping.cropping_radius_max = 15.0 --meters +params.mapper_localizer.scan_to_map_registration.icp.max_correspondence_dist = 2.0 --NOT USED RIGHT NOW +params.mapper_localizer.scan_to_map_registration.icp.knn = 5 --Currently only used for surface normal estimation. +params.mapper_localizer.scan_to_map_registration.icp.max_distance_knn = 1.0 --Currently only used for surface normal estimation. +params.mapper_localizer.scan_to_map_registration.icp.reference_cloud_seting_period = 2.0 --Seconds + +--MAP_INITIALIZER +params.map_initializer.pcd_file_package = "open3d_slam_ros" +params.map_initializer.pcd_file_path = "" --you need to give your own map here. .pcd or .ply well supported. +params.map_initializer.is_initialize_interactively = false +params.map_initializer.init_pose.x = 0.0 +params.map_initializer.init_pose.y = 2.0 +params.map_initializer.init_pose.z = 0.0 +params.map_initializer.init_pose.roll = 0.0 +params.map_initializer.init_pose.pitch = 0.0 +params.map_initializer.init_pose.yaw = 120.0 + +--SUBMAP +params.submap.submap_size = 15.0 --meters +params.submap.adjacency_based_revisiting_min_fitness = 0.5 --How easy we want the system to switch submaps +params.submap.min_seconds_between_feature_computation = 10.0 --seconds, this is a feature of loop-closure +params.submap.submaps_num_scan_overlap = 5 -- 10 -15 +params.submap.max_num_points = 250000 --We dont want our submaps to get too large in dense scenes where distance is a not a indicator of fullness. Hence, we create a submap also based on this upper point limit. + +--MAP_BUILDER +params.map_builder.map_voxel_size = 0.25 --the map voxel size 25cm +params.map_builder.scan_cropping.cropping_radius_max = 40.0 +params.map_builder.scan_cropping.cropping_radius_min = 2.0 +params.map_builder.space_carving.carve_space_every_n_scans = 10 --feature of space carving +params.map_builder.space_carving.max_raytracing_length = 20 --feature of space carving +params.map_builder.space_carving.truncation_distance = 0.3 --feature of space carving +params.map_builder.space_carving.voxel_size = 0.2 --feature of space carving + +--DENSE_MAP_BUILDER +params.dense_map_builder.map_voxel_size = 0.05 +params.dense_map_builder.scan_cropping.cropping_radius_max = 105.0 +params.dense_map_builder.space_carving.carve_space_every_n_scans = 10 +params.dense_map_builder.space_carving.truncation_distance = 0.1 + +--PLACE_RECOGNITION +params.place_recognition.ransac_min_corresondence_set_size = 40 +params.place_recognition.max_icp_correspondence_distance = 1.0 +params.place_recognition.min_icp_refinement_fitness = 0.7 +params.place_recognition.dump_aligned_place_recognitions_to_file = false +params.place_recognition.min_submaps_between_loop_closures = 2 --If you make this 0, you start doing submap-to-submap registration as well. Loop-closure feature has to be enabled for this to take action. +params.place_recognition.loop_closure_search_radius = 20.0 -- The range of search for loop-closures, technically it has to be bigger than the submap size. +params.place_recognition.consistency_check.max_drift_roll = 30.0 --deg +params.place_recognition.consistency_check.max_drift_pitch = 30.0 --deg +params.place_recognition.consistency_check.max_drift_yaw = 30.0 --deg +params.place_recognition.consistency_check.max_drift_x = 80.0 --m +params.place_recognition.consistency_check.max_drift_y = 80.0 --m +params.place_recognition.consistency_check.max_drift_z = 40.0 --m + +--MOTION_COMPENSATION +params.motion_compensation.is_undistort_scan = false --Needs point timestamp field to exist. Not well-tested for summer school. +params.motion_compensation.num_poses_vel_estimation = 3 + +--SAVING +params.saving.save_map = true +params.saving.save_submaps = false --If enabled, the submaps are separately saved. + + +return params \ No newline at end of file diff --git a/smb_slam/launch/online_slam.launch b/smb_slam/launch/online_slam.launch index 18d608b..8353d8d 100644 --- a/smb_slam/launch/online_slam.launch +++ b/smb_slam/launch/online_slam.launch @@ -4,7 +4,7 @@ - + From 278010a0ac5ef077c364d4271c9c6b402148449e Mon Sep 17 00:00:00 2001 From: YifanZhou99 <75839011+YifanZhou99@users.noreply.github.com> Date: Wed, 3 Jul 2024 09:00:38 +0000 Subject: [PATCH 14/31] add dependencies for tf_listener --- team6_stack/CMakeLists.txt | 27 ++++++++++++++++++++------- team6_stack/package.xml | 23 +++++++++++++++++++++++ team6_stack/scripts/tf_listener.py | 16 ++++++++++++++-- 3 files changed, 57 insertions(+), 9 deletions(-) diff --git a/team6_stack/CMakeLists.txt b/team6_stack/CMakeLists.txt index cda589b..ffabe89 100644 --- a/team6_stack/CMakeLists.txt +++ b/team6_stack/CMakeLists.txt @@ -7,9 +7,19 @@ project(team6_stack) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED +find_package(catkin REQUIRED COMPONENTS object_detection - ) + rospy + tf2_ros + geometry_msgs + tf2_geometry_msgs + std_msgs + tf2_msgs + message_generation +) + +catkin_package() + ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -55,6 +65,7 @@ add_message_files( ObjectDetectionInfo.msg ObjectDetectionInfoArray.msg PointCloudArray.msg + TransformedDetection.msg ) ## Generate services in the 'srv' folder @@ -72,10 +83,11 @@ add_message_files( # ) ## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) +generate_messages( + DEPENDENCIES + std_msgs # Or other packages containing msgs + geometry_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -110,6 +122,7 @@ catkin_package( # INCLUDE_DIRS include # LIBRARIES team6_stack # CATKIN_DEPENDS other_catkin_pkg +CATKIN_DEPENDS message_runtime # DEPENDS system_lib ) @@ -121,7 +134,7 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( # include -# ${catkin_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library diff --git a/team6_stack/package.xml b/team6_stack/package.xml index b836ea5..cd528c5 100644 --- a/team6_stack/package.xml +++ b/team6_stack/package.xml @@ -48,8 +48,31 @@ + catkin + rospy + tf2_ros + geometry_msgs + tf2_geometry_msgs + std_msgs + tf2_msgs + + rospy + tf2_ros + geometry_msgs + tf2_geometry_msgs + std_msgs + tf2_msgs + + rospy + tf2_ros + geometry_msgs + tf2_geomerty_msgs + std_msgs + tf2_msgs + + diff --git a/team6_stack/scripts/tf_listener.py b/team6_stack/scripts/tf_listener.py index e246a97..fb704cd 100644 --- a/team6_stack/scripts/tf_listener.py +++ b/team6_stack/scripts/tf_listener.py @@ -4,9 +4,10 @@ import tf2_ros import tf2_msgs from tf2_msgs.msg import TFMessage -from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import TransformStamped, PointStamped import tf2_geometry_msgs from object_detection.object_detection_msgs import ObjectDetectionInfoArray +from my_transform_package.msg import TransformedDetection import numpy as np @@ -24,6 +25,8 @@ translation_cam2opt = None rotation_cam2opt = None +transformed_detection_pub = None + def tf_callback(msg): global translation_odomGraph2worldGraph, rotation_odomGraph2worldGraph @@ -82,6 +85,13 @@ def detection_callback(msg): transformed_pose = tf_buffer.transform(object_pose, "base_link", rospy.Duration(1.0)) transformed_position = transformed_pose.pose.position rospy.loginfo(f"Detected object (class_id: {class_id}) position in base_link frame: x={transformed_position.x}, y={transformed_position.y}, z={transformed_position.z}") + + # Publish the transformed detection + transformed_detection = TransformedDetection() + transformed_detection.position = transformed_position + transformed_detection.class_id = class_id + transformed_detection_pub.publish(transformed_detection) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logerr(f"TF transform error: {e}") @@ -103,7 +113,7 @@ def duplicate_rejection(detections): return unique_detections def main(): - global tf_buffer, tf_listener + global tf_buffer, tf_listener, transformed_detection_pub rospy.init_node('tf_listener', anonymous=True) @@ -114,6 +124,8 @@ def main(): rospy.Subscriber('/tf_static', TFMessage, tf_static_callback) rospy.Subscriber('/object_detector/detection_info', ObjectDetectionInfoArray, detection_callback) + transformed_detection_pub = rospy.Publisher('/transformed_detections', TransformedDetection, queue_size=10) + rospy.Publisher('/artefact_point', PointStamped) rospy.spin() if __name__ == '__main__': From b11fd8e33727a00dfa02a10b70ab3c862f7b0331 Mon Sep 17 00:00:00 2001 From: Neelaksh Singh Date: Wed, 3 Jul 2024 09:32:07 +0000 Subject: [PATCH 15/31] [Rviz vis] Change parameters --- smb_opc/rviz/smb_vis.rviz | 163 ++-- smb_opc/rviz/smb_vis_default.rviz | 1154 +++++++++++++++++++++++++++++ 2 files changed, 1262 insertions(+), 55 deletions(-) create mode 100644 smb_opc/rviz/smb_vis_default.rviz diff --git a/smb_opc/rviz/smb_vis.rviz b/smb_opc/rviz/smb_vis.rviz index df59ac6..dddb4df 100644 --- a/smb_opc/rviz/smb_vis.rviz +++ b/smb_opc/rviz/smb_vis.rviz @@ -15,8 +15,10 @@ Panels: - /FarPlanner1/GoalPoint1/Namespaces1 - /FarPlanner1/GlobalGraph1 - /TarePlanner1 + - /PointCloud21 + - /PointCloud22 Splitter Ratio: 0.5111420750617981 - Tree Height: 770 + Tree Height: 777 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -85,8 +87,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.5905910730361938 - Min Value: -1.1293858289718628 + Max Value: 2.1627650260925293 + Min Value: -1.0256578922271729 Value: true Axis: Z Channel Name: intensity @@ -145,12 +147,10 @@ Visualization Manager: Value: true map_o3d_graph_msf_aligned: Value: true - map_o3d_viz: + odom: Value: true odom_graph_msf: Value: true - odom_o3d: - Value: true raw_rs_o3d: Value: true rgb_camera_link: @@ -178,46 +178,43 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - base_link: - LF_WHEEL: - {} - LH_WHEEL: - {} - RF_WHEEL: - {} - RH_WHEEL: - {} - base_inertia: - {} - imu_link: - imu: + odom: + base_link: + LF_WHEEL: {} - lidar_mount_link: - rslidar_base_link: - rslidar: - map_o3d: - map_o3d_viz: - {} - raw_rs_o3d: - {} - odom_o3d: - {} - sensor: - camera: - {} - vehicle: - {} - odom_graph_msf: - world_graph_msf: - map_o3d_graph_msf_aligned: + LH_WHEEL: + {} + RF_WHEEL: + {} + RH_WHEEL: + {} + base_inertia: + {} + imu_link: + imu: {} - sensor_at_scan: + lidar_mount_link: + rslidar_base_link: + rslidar: + map_o3d: + raw_rs_o3d: + {} + sensor: + camera: + {} + vehicle: + {} + odom_graph_msf: + world_graph_msf: + map_o3d_graph_msf_aligned: + {} + sensor_at_scan: + {} + rgb_camera_link: + rgb_camera_optical_link: {} - rgb_camera_link: - rgb_camera_optical_link: + top: {} - top: - {} Update Interval: 0 Value: true - Alpha: 1 @@ -926,7 +923,7 @@ Visualization Manager: Marker Topic: /sensor_coverage_planner/tare_visualizer/local_planning_horizon Name: LocalPlanningHorizon Namespaces: - "": true + {} Queue Size: 100 Value: true - Class: rviz/Marker @@ -934,7 +931,7 @@ Visualization Manager: Marker Topic: /sensor_coverage_planner/tare_visualizer/exploring_subspaces Name: ExplorationSubspaces Namespaces: - "": true + {} Queue Size: 100 Value: true - Alpha: 1 @@ -1017,13 +1014,69 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - Enabled: true + Enabled: false Name: TarePlanner + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 0; 255; 0 + Min Color: 255; 255; 255 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.30000001192092896 + Style: Boxes + Topic: /sensor_coverage_planner/viewpoint_vis_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + 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/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: /sensor_coverage_planner/planner_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: base_link + Fixed Frame: world_graph_msf Frame Rate: 30 Name: root Tools: @@ -1051,7 +1104,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 46.70221710205078 + Distance: 101.05590057373047 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1059,17 +1112,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 1.7170408964157104 - Y: -3.2570488452911377 - Z: 8.846016883850098 + X: -0.06670406460762024 + Y: 12.508928298950195 + Z: 2.5416529178619385 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7397965788841248 + Pitch: 0.9747965335845947 Target Frame: - Yaw: 3.385552406311035 + Yaw: 3.390552043914795 Saved: ~ Window Geometry: ControlPanel: @@ -1081,7 +1134,7 @@ Window Geometry: Hide Right Dock: true Image bounding Boxes: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000002d000000542fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d000003a2000000fb0100001cfa000000010100000002fb000000180043006f006e00740072006f006c00500061006e0065006c0100000000ffffffff000000d200fffffffb000000100044006900730070006c0061007900730100000000000001ea0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000003e5000000b70000007300fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f0061006401000004a2000000dd0000007300ffffff00000001000001c3000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a400fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000001600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070f0000005efc0100000002fb0000000800540069006d006501000000000000070f000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004390000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002d000000546fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003b000003a5000000ed0100001afa000000010100000002fb000000180043006f006e00740072006f006c00500061006e0065006c0100000000ffffffff000000bf00fffffffb000000100044006900730070006c0061007900730100000000000001ea0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000003e6000000b70000006d00fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f0061006401000004a3000000de0000006d00ffffff00000001000001c3000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a000fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000001600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070f0000005efc0100000002fb0000000800540069006d006501000000000000070f0000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000004390000054600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Raw Image: collapsed: false SMBPowerMotor: @@ -1097,5 +1150,5 @@ Window Geometry: Views: collapsed: true Width: 1807 - X: 0 - Y: 27 + X: -1 + Y: 3 diff --git a/smb_opc/rviz/smb_vis_default.rviz b/smb_opc/rviz/smb_vis_default.rviz new file mode 100644 index 0000000..8fca1c3 --- /dev/null +++ b/smb_opc/rviz/smb_vis_default.rviz @@ -0,0 +1,1154 @@ +Panels: + - Class: rviz/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Robot1 + - /Robot1/Sensors1/Lidar Raw1 + - /SLAM1/Map1 + - /SLAM1/Map1/Autocompute Value Bounds1 + - /LocalPlanner1 + - /LocalPlanner1/TerrainMap1 + - /FarPlanner1 + - /FarPlanner1/GoalPoint1/Namespaces1 + - /FarPlanner1/GlobalGraph1 + - /TarePlanner1 + - /PointCloud21 + - /PointCloud22 + Splitter Ratio: 0.5111420750617981 + Tree Height: 777 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + - /Waypoint1 + - /Goalpoint1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: Lidar Raw + - Class: rviz/ControlPanel + Name: ControlPanel +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/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: + Value: true + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: smb_rviz_plugins/SMBPowerMotor + Enabled: true + Name: SMBPowerMotor + Topic Power Motor: /smb_powerstatus/base + Value: true + - Class: smb_rviz_plugins/SMBPowerPayload + Enabled: true + Name: SMBPowerPayload + Show/Hide: + Show Battery 1: true + Show Battery 2: true + Show Plug: true + Topic Power Payload: /smb_powerstatus/payload + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.158384323120117 + Min Value: -1.0142515897750854 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Lidar Raw + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.029999999329447746 + Style: Flat Squares + Topic: /rslidar/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Sensors + - Class: rviz/Group + Displays: + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + LF_WHEEL: + Value: true + LH_WHEEL: + Value: true + RF_WHEEL: + Value: true + RH_WHEEL: + Value: true + base_inertia: + Value: true + base_link: + Value: true + camera: + Value: true + imu: + Value: true + imu_link: + Value: true + lidar_mount_link: + Value: true + map_o3d: + Value: true + map_o3d_graph_msf_aligned: + Value: true + odom: + Value: true + odom_graph_msf: + Value: true + raw_rs_o3d: + Value: true + rgb_camera_link: + Value: true + rgb_camera_optical_link: + Value: true + rslidar: + Value: true + rslidar_base_link: + Value: true + sensor: + Value: true + sensor_at_scan: + Value: true + top: + Value: true + vehicle: + Value: true + world_graph_msf: + Value: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + odom: + base_link: + LF_WHEEL: + {} + LH_WHEEL: + {} + RF_WHEEL: + {} + RH_WHEEL: + {} + base_inertia: + {} + imu_link: + imu: + {} + lidar_mount_link: + rslidar_base_link: + rslidar: + map_o3d: + raw_rs_o3d: + {} + sensor: + camera: + {} + vehicle: + {} + odom_graph_msf: + world_graph_msf: + map_o3d_graph_msf_aligned: + {} + sensor_at_scan: + {} + rgb_camera_link: + rgb_camera_optical_link: + {} + top: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + LF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rgb_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rgb_camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rslidar: + Alpha: 1 + Show Axes: false + Show Trail: false + rslidar_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: smb_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Name: Description + Enabled: true + Name: Robot + - Class: rviz/Group + Displays: + - Alpha: 1 + Angular Arrow Scale: 2 + Angular Color: 204; 204; 51 + Arrow Width: 0.5 + Class: rviz/TwistStamped + Enabled: true + Hide Small Values: true + History Length: 1 + Linear Arrow Scale: 2 + Linear Color: 204; 51; 51 + Name: Command Twist + Queue Size: 10 + Topic: /command_twist_stamped + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: false + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Current Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /current_pose + Unreliable: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 206; 92; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.10000000149011612 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Optimal Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.009999999776482582 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /optimal_path + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: false + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Optimal Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /optimal_pose + Unreliable: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.10000000149011612 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: MPC Reference Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.009999999776482582 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /mpc_trajectory_standalone + Unreliable: false + Value: true + Enabled: true + Name: MPC + - Class: rviz/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: GlobalCostmap + Topic: /move_base/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: LocalCostmap + Topic: /move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: GlobalPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/TebLocalPlannerROS/global_plan + Unreliable: false + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: LocalPath + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /move_base/TebLocalPlannerROS/teb_poses + Unreliable: false + Value: true + Enabled: true + Name: PathPlanner + - Class: rviz/Group + Displays: + - Alpha: 0.800000011920929 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.6000000238418579 + Min Value: -4 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Map + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.05000000074505806 + Style: Points + Topic: /mapping/assembled_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: SLAM + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 100 + Name: WheelOdometry + Position Tolerance: 0.10000000149011612 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /control/smb_diff_drive/odom + Unreliable: false + Value: false + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 100 + Name: LidarOdometry + Position Tolerance: 0.10000000149011612 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /graph_msf/est_odometry_odom_imu + Unreliable: false + Value: true + Enabled: true + Name: StateEstimation + - Class: rviz/Group + Displays: + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /explore/frontiers + Name: Frontiers + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /exploration_map/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + Enabled: true + Name: Exploration + - Class: rviz/Group + Displays: + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.5 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: 3D Object Poses + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Axes + Topic: /object_detector/object_poses + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /object_detector/detections_in_image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image bounding Boxes + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /rgb_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Raw Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Object Detection + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Trajectory + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /trajectory + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 6 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LocalFreePaths + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /free_paths + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: WayPoint + Queue Size: 10 + Radius: 1 + Topic: /way_point + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 0.10000000149011612 + Min Color: 0; 0; 0 + Min Intensity: -0.10000000149011612 + Name: TerrainMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.10000000149011612 + Style: Spheres + Topic: /terrain_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 0.20000000298023224 + Min Color: 0; 0; 0 + Min Intensity: -0.20000000298023224 + Name: TerrainMapExt + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.05000000074505806 + Style: Points + Topic: /terrain_map_ext + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: LocalPlannerPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /path + Unreliable: false + Value: true + Enabled: true + Name: LocalPlanner + - Class: rviz/Group + Displays: + - Alpha: 1 + Class: rviz/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: GoalPointIn + Queue Size: 10 + Radius: 0.20000000298023224 + Topic: /goal_point + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /viz_node_topic + Name: GoalPoint + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /viz_graph_topic + Name: GlobalGraph + Namespaces: + {} + Queue Size: 100 + 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/PointCloud2 + Color: 245; 121; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: ObsPoints + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.4000000059604645 + Style: Spheres + Topic: /FAR_obs_debug + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: DynamicObs + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /FAR_dynamic_obs_debug + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /viz_path_topic + Name: GlobalPath + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: FarPlanner + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.30000001192092896 + Name: TareLocalPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /sensor_coverage_planner/local_path + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /sensor_coverage_planner/tare_visualizer/local_planning_horizon + Name: LocalPlanningHorizon + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /sensor_coverage_planner/tare_visualizer/exploring_subspaces + Name: ExplorationSubspaces + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 98; 240; 231 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.20000000298023224 + Name: TareGlobalPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /sensor_coverage_planner/global_path + Unreliable: false + 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/PointCloud2 + Color: 237; 212; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: ViewPointCandidates + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 1 + Style: Spheres + Topic: /sensor_coverage_planner/viewpoint_vis_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 245; 121; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: SelectedViewPoints + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 1 + Style: Spheres + Topic: /sensor_coverage_planner/selected_viewpoint_vis_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: TarePlanner + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 0; 255; 0 + Min Color: 255; 255; 255 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.30000001192092896 + Style: Boxes + Topic: /sensor_coverage_planner/viewpoint_vis_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + 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/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: /sensor_coverage_planner/planner_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world_graph_msf + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + - Class: rviz/WaypointTool + Topic: waypoint + - Class: rviz/GoalPointTool + Topic: goalpoint + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 101.05590057373047 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -0.06670406460762024 + Y: 12.508928298950195 + Z: 2.5416529178619385 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9747965335845947 + Target Frame: + Yaw: 3.390552043914795 + Saved: ~ +Window Geometry: + ControlPanel: + collapsed: false + Displays: + collapsed: false + Height: 1536 + Hide Left Dock: false + Hide Right Dock: true + Image bounding Boxes: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000002d000000546fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003b000003a5000000ed0100001afa000000010100000002fb000000180043006f006e00740072006f006c00500061006e0065006c0100000000ffffffff000000bf00fffffffb000000100044006900730070006c0061007900730100000000000001ea0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000003e6000000b70000006d00fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f0061006401000004a3000000de0000006d00ffffff00000001000001c3000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a000fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000001600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070f0000005efc0100000002fb0000000800540069006d006501000000000000070f0000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000004390000054600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Raw Image: + collapsed: false + SMBPowerMotor: + collapsed: false + SMBPowerPayload: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1807 + X: -1 + Y: 3 From 0d42c6c0de2e3f86860e46fcdd8b14c6503f58fa Mon Sep 17 00:00:00 2001 From: Alexander James Becoy Date: Wed, 3 Jul 2024 09:49:28 +0000 Subject: [PATCH 16/31] Create ROS node to inspect detected objects --- team6_stack/CMakeLists.txt | 12 ++-- team6_stack/scripts/object_inspection.py | 90 ++++++++++++++++++++++++ 2 files changed, 96 insertions(+), 6 deletions(-) create mode 100644 team6_stack/scripts/object_inspection.py diff --git a/team6_stack/CMakeLists.txt b/team6_stack/CMakeLists.txt index 6897b2f..657b0e8 100644 --- a/team6_stack/CMakeLists.txt +++ b/team6_stack/CMakeLists.txt @@ -153,12 +153,12 @@ include_directories( # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +# Mark executable scripts (Python etc.) for installation +# in contrast to setup.py, you can choose the destination +catkin_install_python(PROGRAMS + scripts/object_inspection.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html diff --git a/team6_stack/scripts/object_inspection.py b/team6_stack/scripts/object_inspection.py new file mode 100644 index 0000000..fbe4fcd --- /dev/null +++ b/team6_stack/scripts/object_inspection.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python + +import rospy +import tf2_ros +import tf2_msgs +from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import PointStamped, PoseStamped +import tf2_geometry_msgs + +import numpy as np + +eps = 1.0 # m, minimum distance between two artefacts +min_dist = 2.0 # m, minimum distance from artefact to robot +timer_period = 0.01 # s, timer period + +""" @TODO: +- Go to artefact with some distance from it +- Ensure that the waypoint is reachable and not in collision +- How to handle multiple artefacts? +- When to remove artefact from the list? + - Inspect object from multiple valid angles +""" + +class ObjectInspectionNode(object): + def __init__(self) -> None: + rospy.init_node('my_node') + rospy.on_shutdown(self.shutdown) + + self.artefacts = [] # list of artefacts detected in PointStamped format + + self.timer = rospy.Timer(rospy.Duration(0, int(timer_period*1e9)), self.timer_callback) + self.artefact_sub = rospy.Subscriber('/artefact_point', PointStamped, self.artefact_point_callback) + self.waypoint_pub = rospy.Publisher('/way_point', PointStamped, queue_size=1) + + def run(self) -> None: + rospy.spin() + + def shutdown(self) -> None: + rospy.loginfo("Shutting down...") + + def artefact_point_callback(self, msg: PointStamped) -> None: + # Check if the artefact is already in the list + for artefact in self.artefacts: + if self.distance(artefact, msg) < eps: + rospy.loginfo_throttle(10, "Duplicate artefact detected") + return + self.artefacts.append(msg) + rospy.loginfo(f"Artefact detected: x={msg.point.x}, y={msg.point.y}, z={msg.point.z}") + + def timer_callback(self, event) -> None: + # Check if there are any artefacts + if not self.artefacts: + return + + # Get the current robot position + tf_buffer = tf2_ros.Buffer() + tf_listener = tf2_ros.TransformListener(tf_buffer) + try: + transform = tf_buffer.lookup_transform('base_link', 'world_msf_graph', rospy.Time()) + robot_position = transform.transform.translation + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): + rospy.logwarn("Failed to get robot position") + return + + # Check if the robot is close enough to the artefact + artefact = self.artefacts[0] + if self.distance(robot_position, artefact) < min_dist: + rospy.loginfo("Robot is close to the artefact") + self.artefacts.pop(0) + else: + # Publish the artefact as a waypoint + self.waypoint_pub.publish(artefact) + + def distance(self, p1: PointStamped, p2: PointStamped) -> float: + return np.sqrt( + (p1.point.x - p2.point.x)**2 + \ + (p1.point.y - p2.point.y)**2 + \ + (p1.point.z - p2.point.z)**2 + ) + +def main() -> None: + try: + node = ObjectInspectionNode() + node.run() + except rospy.ROSInterruptException: + pass + + +if __name__ == '__main__': + main() \ No newline at end of file From a32fabee8697a653b18406a9fe69a853b5cb499b Mon Sep 17 00:00:00 2001 From: YifanZhou99 <75839011+YifanZhou99@users.noreply.github.com> Date: Wed, 3 Jul 2024 09:56:34 +0000 Subject: [PATCH 17/31] fixed cmak e and pkg --- team6_stack/CMakeLists.txt | 29 ++++++++--------------------- team6_stack/package.xml | 25 +------------------------ 2 files changed, 9 insertions(+), 45 deletions(-) diff --git a/team6_stack/CMakeLists.txt b/team6_stack/CMakeLists.txt index ffabe89..3c048ab 100644 --- a/team6_stack/CMakeLists.txt +++ b/team6_stack/CMakeLists.txt @@ -7,19 +7,9 @@ project(team6_stack) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS +find_package(catkin REQUIRED object_detection - rospy - tf2_ros - geometry_msgs - tf2_geometry_msgs - std_msgs - tf2_msgs - message_generation -) - -catkin_package() - + ) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -65,7 +55,6 @@ add_message_files( ObjectDetectionInfo.msg ObjectDetectionInfoArray.msg PointCloudArray.msg - TransformedDetection.msg ) ## Generate services in the 'srv' folder @@ -83,11 +72,10 @@ add_message_files( # ) ## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs # Or other packages containing msgs - geometry_msgs -) +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -122,7 +110,6 @@ catkin_package( # INCLUDE_DIRS include # LIBRARIES team6_stack # CATKIN_DEPENDS other_catkin_pkg -CATKIN_DEPENDS message_runtime # DEPENDS system_lib ) @@ -134,7 +121,7 @@ CATKIN_DEPENDS message_runtime ## Your package locations should be listed before other locations include_directories( # include - ${catkin_INCLUDE_DIRS} +# ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library @@ -220,4 +207,4 @@ catkin_install_python(PROGRAMS # endif() ## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +# catkin_add_nosetests(test) \ No newline at end of file diff --git a/team6_stack/package.xml b/team6_stack/package.xml index cd528c5..20c3c6c 100644 --- a/team6_stack/package.xml +++ b/team6_stack/package.xml @@ -48,35 +48,12 @@ - catkin - rospy - tf2_ros - geometry_msgs - tf2_geometry_msgs - std_msgs - tf2_msgs - - rospy - tf2_ros - geometry_msgs - tf2_geometry_msgs - std_msgs - tf2_msgs - - rospy - tf2_ros - geometry_msgs - tf2_geomerty_msgs - std_msgs - tf2_msgs - - - + \ No newline at end of file From 15dfd427a48d7fbb170bebde5d1c8d131240fac4 Mon Sep 17 00:00:00 2001 From: Neelaksh Singh Date: Wed, 3 Jul 2024 10:04:27 +0000 Subject: [PATCH 18/31] rviz profile with exploration --- smb_opc/rviz/smb_vis.rviz | 18 +++- smb_opc/rviz/smb_vis_default.rviz | 163 ++++++++++-------------------- 2 files changed, 68 insertions(+), 113 deletions(-) diff --git a/smb_opc/rviz/smb_vis.rviz b/smb_opc/rviz/smb_vis.rviz index 109bed4..8fca1c3 100644 --- a/smb_opc/rviz/smb_vis.rviz +++ b/smb_opc/rviz/smb_vis.rviz @@ -87,8 +87,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 2.1627650260925293 - Min Value: -1.0256578922271729 + Max Value: 2.158384323120117 + Min Value: -1.0142515897750854 Value: true Axis: Z Channel Name: intensity @@ -135,6 +135,8 @@ Visualization Manager: Value: true base_link: Value: true + camera: + Value: true imu: Value: true imu_link: @@ -159,8 +161,16 @@ Visualization Manager: Value: true rslidar_base_link: Value: true + sensor: + Value: true + sensor_at_scan: + Value: true top: Value: true + vehicle: + Value: true + world_graph_msf: + Value: true Marker Alpha: 1 Marker Scale: 1 Name: TF @@ -914,7 +924,6 @@ Visualization Manager: Name: LocalPlanningHorizon Namespaces: {} - {} Queue Size: 100 Value: true - Class: rviz/Marker @@ -923,7 +932,6 @@ Visualization Manager: Name: ExplorationSubspaces Namespaces: {} - {} Queue Size: 100 Value: true - Alpha: 1 @@ -1121,7 +1129,7 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1136 + Height: 1536 Hide Left Dock: false Hide Right Dock: true Image bounding Boxes: diff --git a/smb_opc/rviz/smb_vis_default.rviz b/smb_opc/rviz/smb_vis_default.rviz index 8fca1c3..df59ac6 100644 --- a/smb_opc/rviz/smb_vis_default.rviz +++ b/smb_opc/rviz/smb_vis_default.rviz @@ -15,10 +15,8 @@ Panels: - /FarPlanner1/GoalPoint1/Namespaces1 - /FarPlanner1/GlobalGraph1 - /TarePlanner1 - - /PointCloud21 - - /PointCloud22 Splitter Ratio: 0.5111420750617981 - Tree Height: 777 + Tree Height: 770 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -87,8 +85,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 2.158384323120117 - Min Value: -1.0142515897750854 + Max Value: 1.5905910730361938 + Min Value: -1.1293858289718628 Value: true Axis: Z Channel Name: intensity @@ -147,10 +145,12 @@ Visualization Manager: Value: true map_o3d_graph_msf_aligned: Value: true - odom: + map_o3d_viz: Value: true odom_graph_msf: Value: true + odom_o3d: + Value: true raw_rs_o3d: Value: true rgb_camera_link: @@ -178,43 +178,46 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - odom: - base_link: - LF_WHEEL: - {} - LH_WHEEL: - {} - RF_WHEEL: - {} - RH_WHEEL: - {} - base_inertia: + base_link: + LF_WHEEL: + {} + LH_WHEEL: + {} + RF_WHEEL: + {} + RH_WHEEL: + {} + base_inertia: + {} + imu_link: + imu: {} - imu_link: - imu: - {} - lidar_mount_link: - rslidar_base_link: - rslidar: - map_o3d: - raw_rs_o3d: - {} - sensor: - camera: - {} - vehicle: - {} - odom_graph_msf: - world_graph_msf: - map_o3d_graph_msf_aligned: - {} - sensor_at_scan: + lidar_mount_link: + rslidar_base_link: + rslidar: + map_o3d: + map_o3d_viz: + {} + raw_rs_o3d: + {} + odom_o3d: {} - rgb_camera_link: - rgb_camera_optical_link: + sensor: + camera: + {} + vehicle: + {} + odom_graph_msf: + world_graph_msf: + map_o3d_graph_msf_aligned: {} - top: + sensor_at_scan: + {} + rgb_camera_link: + rgb_camera_optical_link: {} + top: + {} Update Interval: 0 Value: true - Alpha: 1 @@ -923,7 +926,7 @@ Visualization Manager: Marker Topic: /sensor_coverage_planner/tare_visualizer/local_planning_horizon Name: LocalPlanningHorizon Namespaces: - {} + "": true Queue Size: 100 Value: true - Class: rviz/Marker @@ -931,7 +934,7 @@ Visualization Manager: Marker Topic: /sensor_coverage_planner/tare_visualizer/exploring_subspaces Name: ExplorationSubspaces Namespaces: - {} + "": true Queue Size: 100 Value: true - Alpha: 1 @@ -1014,69 +1017,13 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - Enabled: false - Name: TarePlanner - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 Enabled: true - Invert Rainbow: false - Max Color: 0; 255; 0 - Min Color: 255; 255; 255 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.30000001192092896 - Style: Boxes - Topic: /sensor_coverage_planner/viewpoint_vis_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: false - 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/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.20000000298023224 - Style: Flat Squares - Topic: /sensor_coverage_planner/planner_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true + Name: TarePlanner Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: world_graph_msf + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: @@ -1104,7 +1051,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 101.05590057373047 + Distance: 46.70221710205078 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1112,17 +1059,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: -0.06670406460762024 - Y: 12.508928298950195 - Z: 2.5416529178619385 + X: 1.7170408964157104 + Y: -3.2570488452911377 + Z: 8.846016883850098 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.9747965335845947 + Pitch: 0.7397965788841248 Target Frame: - Yaw: 3.390552043914795 + Yaw: 3.385552406311035 Saved: ~ Window Geometry: ControlPanel: @@ -1134,7 +1081,7 @@ Window Geometry: Hide Right Dock: true Image bounding Boxes: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000002d000000546fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003b000003a5000000ed0100001afa000000010100000002fb000000180043006f006e00740072006f006c00500061006e0065006c0100000000ffffffff000000bf00fffffffb000000100044006900730070006c0061007900730100000000000001ea0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000003e6000000b70000006d00fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f0061006401000004a3000000de0000006d00ffffff00000001000001c3000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a000fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000001600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070f0000005efc0100000002fb0000000800540069006d006501000000000000070f0000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000004390000054600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002d000000542fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d000003a2000000fb0100001cfa000000010100000002fb000000180043006f006e00740072006f006c00500061006e0065006c0100000000ffffffff000000d200fffffffb000000100044006900730070006c0061007900730100000000000001ea0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000003e5000000b70000007300fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f0061006401000004a2000000dd0000007300ffffff00000001000001c3000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a400fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000001600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070f0000005efc0100000002fb0000000800540069006d006501000000000000070f000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004390000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Raw Image: collapsed: false SMBPowerMotor: @@ -1150,5 +1097,5 @@ Window Geometry: Views: collapsed: true Width: 1807 - X: -1 - Y: 3 + X: 0 + Y: 27 From f9ab57bd36c902f9b50ec6e067ee06ef50a0ba4a Mon Sep 17 00:00:00 2001 From: Magdalena Yordanova Date: Wed, 3 Jul 2024 10:07:11 +0000 Subject: [PATCH 19/31] saving object detection to a csv file --- .../data/artifacts-20240703-100156.csv | 9 ++++ team6_stack/scripts/team6_stack.py | 43 ++++++++++++++++--- 2 files changed, 45 insertions(+), 7 deletions(-) create mode 100644 team6_stack/data/artifacts-20240703-100156.csv diff --git a/team6_stack/data/artifacts-20240703-100156.csv b/team6_stack/data/artifacts-20240703-100156.csv new file mode 100644 index 0000000..725b31f --- /dev/null +++ b/team6_stack/data/artifacts-20240703-100156.csv @@ -0,0 +1,9 @@ +class_id,x,y,z +tv,0.903783752524338,-0.1496109038050454,6.6691977996826175 +stop sign,-0.9870599908911698,-1.0336732224398708,4.568266294805018 +tv,1.0044713696202927,-0.14900861621552972,6.664283039093018 +stop sign,-0.9864215472345935,-1.0337180179481715,4.56376327728644 +tv,1.00362096483457,-0.14918880414486493,6.6586763877868655 +stop sign,-0.9866787940274824,-1.0335090997750394,4.565991839711263 +tv,1.0045056362909914,-0.1488167659252669,6.664487602233887 +stop sign,-0.908534771975907,-1.0091440334423676,4.566319019782837 diff --git a/team6_stack/scripts/team6_stack.py b/team6_stack/scripts/team6_stack.py index aae2e3e..438fdaa 100755 --- a/team6_stack/scripts/team6_stack.py +++ b/team6_stack/scripts/team6_stack.py @@ -1,10 +1,14 @@ #!/usr/bin/env python import rospy +import os +import csv +import time + from std_msgs.msg import Header from object_detection_msgs.msg import ObjectDetectionInfoArray -# TODO: needs to be tested +timestr = '' def callback(msg): infos = [] @@ -19,15 +23,40 @@ def callback(msg): print(infos) - # Save the detected artifacts to a csv file - with open('data/artifacts.csv', mode='a', newline='') as file: - writer = csv.writer(file) - for info in infos: - writer.writerow([info['class_id'], info['x'], info['y'], info['z']]) - + # Check if the CSV file exists + dirname = os.path.dirname(__file__) + #timestr = time.strftime("%Y%m%d-%H%M%S") + file_name = 'artifacts-' + timestr + '.csv' # Add the already set timestamp + #file_name = 'artifacts.csv' + file_path = os.path.join(dirname, './../data/' + file_name) # Relative path + file_exists = os.path.isfile(file_path) + + # Save the detected artifacts to a CSV file + if file_exists: + with open(file_path, mode='a', newline='') as file: + writer = csv.writer(file) + for info in infos: + writer.writerow([info['class_id'], info['x'], info['y'], info['z']]) + def object_detection_listener(): rospy.init_node('team6_stack', anonymous=True) + + global timestr + # Check if the CSV file exists + dirname = os.path.dirname(__file__) + timestr = time.strftime("%Y%m%d-%H%M%S") + #file_name = 'artifacts.csv' + file_name = 'artifacts-' + timestr + '.csv' # Add timestamp for the beginning of the run + file_path = os.path.join(dirname, './../data/' + file_name) # Relative path + file_exists = os.path.isfile(file_path) + + # Write the header only if the file doesn't exist + if not file_exists: + with open(file_path, mode='w', newline='') as file: + writer = csv.writer(file) + writer.writerow(['class_id', 'x', 'y', 'z']) + # Subscribe to the /object_detector/detection_info topic rospy.Subscriber('/object_detector/detection_info', ObjectDetectionInfoArray, callback) From 8c19e14ca8e5d200b206b917e600dcc0fb5c3c37 Mon Sep 17 00:00:00 2001 From: YifanZhou99 <75839011+YifanZhou99@users.noreply.github.com> Date: Wed, 3 Jul 2024 10:07:45 +0000 Subject: [PATCH 20/31] fixed again --- team6_stack/CMakeLists.txt | 30 ++++++++++++++++-------------- team6_stack/package.xml | 1 + 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/team6_stack/CMakeLists.txt b/team6_stack/CMakeLists.txt index 3c048ab..999be7c 100644 --- a/team6_stack/CMakeLists.txt +++ b/team6_stack/CMakeLists.txt @@ -7,9 +7,11 @@ project(team6_stack) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED - object_detection - ) +find_package(catkin REQUIRED COMPONENTS + rospy + object_detection_msgs + object_detection +) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -45,17 +47,17 @@ find_package(catkin REQUIRED ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -add_message_files( - DIRECTORY msg - FILES - ObjectDetection.msg - ObjectDetectionArray.msg - ObjectPosition.msg - ObjectPositionArray.msg - ObjectDetectionInfo.msg - ObjectDetectionInfoArray.msg - PointCloudArray.msg -) +# add_message_files( +# DIRECTORY msg +# FILES +# ObjectDetection.msg +# ObjectDetectionArray.msg +# ObjectPosition.msg +# ObjectPositionArray.msg +# ObjectDetectionInfo.msg +# ObjectDetectionInfoArray.msg +# PointCloudArray.msg +# ) ## Generate services in the 'srv' folder # add_service_files( diff --git a/team6_stack/package.xml b/team6_stack/package.xml index 20c3c6c..8228089 100644 --- a/team6_stack/package.xml +++ b/team6_stack/package.xml @@ -49,6 +49,7 @@ catkin + object_detection From 85d5141cf0867c32451d79cb35f4228c27145187 Mon Sep 17 00:00:00 2001 From: Neelaksh Singh Date: Wed, 3 Jul 2024 10:08:17 +0000 Subject: [PATCH 21/31] tmux session save file --- tmux_session.sh | 58 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) create mode 100644 tmux_session.sh diff --git a/tmux_session.sh b/tmux_session.sh new file mode 100644 index 0000000..287c0cc --- /dev/null +++ b/tmux_session.sh @@ -0,0 +1,58 @@ +#!/usr/bin/env bash +# Save and restore the state of tmux sessions and windows. +# TODO: persist and restore the state & position of panes. +set -e + +dump() { + local d=$'\t' + tmux list-windows -a -F "#S${d}#W${d}#{pane_current_path}" +} + +save() { + dump > ~/.tmux-session +} + +terminal_size() { + stty size 2>/dev/null | awk '{ printf "-x%d -y%d", $2, $1 }' +} + +session_exists() { + tmux has-session -t "$1" 2>/dev/null +} + +add_window() { + tmux new-window -d -t "$1:" -n "$2" -c "$3" +} + +new_session() { + cd "$3" && + tmux new-session -d -s "$1" -n "$2" $4 +} + +restore() { + tmux start-server + local count=0 + local dimensions="$(terminal_size)" + + while IFS=$'\t' read session_name window_name dir; do + if [[ -d "$dir" && $window_name != "log" && $window_name != "man" ]]; then + if session_exists "$session_name"; then + add_window "$session_name" "$window_name" "$dir" + else + new_session "$session_name" "$window_name" "$dir" "$dimensions" + count=$(( count + 1 )) + fi + fi + done < ~/.tmux-session + + echo "restored $count sessions" +} + +case "$1" in +save | restore ) + $1 + ;; +* ) + echo "valid commands: save, restore" >&2 + exit 1 +esac \ No newline at end of file From 93fad7dd7e52e322b2f3e9fe4ed1a6370abd3d77 Mon Sep 17 00:00:00 2001 From: YifanZhou99 <75839011+YifanZhou99@users.noreply.github.com> Date: Wed, 3 Jul 2024 10:44:28 +0000 Subject: [PATCH 22/31] new branch --- team6_stack/scripts/tf_listener.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/team6_stack/scripts/tf_listener.py b/team6_stack/scripts/tf_listener.py index fb704cd..72b0d1d 100644 --- a/team6_stack/scripts/tf_listener.py +++ b/team6_stack/scripts/tf_listener.py @@ -125,7 +125,7 @@ def main(): rospy.Subscriber('/object_detector/detection_info', ObjectDetectionInfoArray, detection_callback) transformed_detection_pub = rospy.Publisher('/transformed_detections', TransformedDetection, queue_size=10) - rospy.Publisher('/artefact_point', PointStamped) + #rospy.Publisher('/artefact_point', PointStamped) rospy.spin() if __name__ == '__main__': From fadfe93daed3e64bc3b6580239483d2d4ad3a04e Mon Sep 17 00:00:00 2001 From: YifanZhou99 <75839011+YifanZhou99@users.noreply.github.com> Date: Wed, 3 Jul 2024 11:58:59 +0000 Subject: [PATCH 23/31] tf_listener published pointstamped, tf tbd --- team6_stack/scripts/tf_listener.py | 91 ++++++++++++++++++------------ 1 file changed, 55 insertions(+), 36 deletions(-) diff --git a/team6_stack/scripts/tf_listener.py b/team6_stack/scripts/tf_listener.py index 72b0d1d..b098ad1 100644 --- a/team6_stack/scripts/tf_listener.py +++ b/team6_stack/scripts/tf_listener.py @@ -6,8 +6,10 @@ from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped, PointStamped import tf2_geometry_msgs -from object_detection.object_detection_msgs import ObjectDetectionInfoArray -from my_transform_package.msg import TransformedDetection + +from object_detection_msgs.msg import ObjectDetectionInfoArray + +#from my_transform_package.msg import TransformedDetection import numpy as np @@ -25,7 +27,9 @@ translation_cam2opt = None rotation_cam2opt = None -transformed_detection_pub = None + +point_stamped = None +artefact_point_pub = rospy.Publisher('/artefact_point', PointStamped, queue_size=10) def tf_callback(msg): @@ -35,18 +39,22 @@ def tf_callback(msg): for transform in msg.transforms: # world -> base_link - if transform.header.frame_id == "odom_graph_msf" and transform.child_frame_id == "world_graph_msf" + if transform.header.frame_id == "odom_graph_msf" and transform.child_frame_id == "world_graph_msf": translation_odomGraph2worldGraph = transform.transform.translation rotation_odomGraph2worldGraph = transform.transform.rotation - if transfrom.header.frame_id == "base_link" and transform.child_frame_id == "odom_graph_msf": + #rospy.loginfo(f"Translation_odomGraph2worldGraph: x={translation_odomGraph2worldGraph.x}, y={translation_odomGraph2worldGraph.y}, z={translation_odomGraph2worldGraph.z}") + #rospy.loginfo(f"Rotation_odomGraph2worldGraph: x={rotation_odomGraph2worldGraph.x}, y={rotation_odomGraph2worldGraph.y}, z={rotation_odomGraph2worldGraph.z}, w={rotation_odomGraph2worldGraph.w} ") + if transform.header.frame_id == "base_link" and transform.child_frame_id == "odom_graph_msf": translation_base2odomGraph = transform.transform.translation rotation_base2odomGraph = transform.transform.rotation + #rospy.loginfo(f"Translation_base2odomGraph: x={translation_base2odomGraph.x}, y={translation_base2odomGraph.y}, z={translation_base2odomGraph.z}") + #rospy.loginfo(f"Rotation_base2odomGraph: x={rotation_base2odomGraph.x}, y={rotation_base2odomGraph.y}, z={rotation_base2odomGraph.z}, w={rotation_base2odomGraph.w}") if transform.header.frame_id == "odom" and transform.child_frame_id == "base_link": translation_world2base = transform.transform.translation rotation_world2base = transform.transform.rotation - rospy.loginfo(f"Translation_world2base: x={translation_world2base.x}, y={translation_world2base.y}, z={translation_world2base.z}") - rospy.loginfo(f"Rotation_world2base: x={rotation_world2base.x}, y={rotation_world2base.y}, z={rotation_world2base.z}, w={rotation_world2base.w}") + #rospy.loginfo(f"Translation_world2base: x={translation_world2base.x}, y={translation_world2base.y}, z={translation_world2base.z}") + #rospy.loginfo(f"Rotation_world2base: x={rotation_world2base.x}, y={rotation_world2base.y}, z={rotation_world2base.z}, w={rotation_world2base.w}") def tf_static_callback(msg): @@ -58,42 +66,52 @@ def tf_static_callback(msg): if transform.header.frame_id == "base_link" and transform.child_frame_id == "rgb_camera_link": translation_base2cam = transform.transform.translation rotation_base2cam = transform.transform.rotation - rospy.loginfo(f"Translation_base2cam: x={translation_base2cam.x}, y={translation_base2cam.y}, z={translation_base2cam.z}") - rospy.loginfo(f"Rotation_base2cam: x={rotation_base2cam.x}, y={rotation_base2cam.y}, z={rotation_base2cam.z}, w={rotation_base2cam.w}") + #rospy.loginfo(f"Translation_base2cam: x={translation_base2cam.x}, y={translation_base2cam.y}, z={translation_base2cam.z}") + #rospy.loginfo(f"Rotation_base2cam: x={rotation_base2cam.x}, y={rotation_base2cam.y}, z={rotation_base2cam.z}, w={rotation_base2cam.w}") # rgb_camera_link -> rgb_camera_optical_link if transform.header.frame_id == "rgb_camera_link" and transform.child_frame_id == "rgb_camera_optical_link": translation_cam2opt = transform.transform.translation rotation_cam2opt = transform.transform.rotation - rospy.loginfo(f"Translation_cam2opt: x={translation_cam2opt.x}, y={translation_cam2opt.y}, z={translation_cam2opt.z}") - rospy.loginfo(f"Rotation_cam2opt: x={rotation_cam2opt.x}, y={rotation_cam2opt.y}, z={rotation_cam2opt.z}, w={rotation_cam2opt.w}") + #rospy.loginfo(f"Translation_cam2opt: x={translation_cam2opt.x}, y={translation_cam2opt.y}, z={translation_cam2opt.z}") + #rospy.loginfo(f"Rotation_cam2opt: x={rotation_cam2opt.x}, y={rotation_cam2opt.y}, z={rotation_cam2opt.z}, w={rotation_cam2opt.w}") def detection_callback(msg): + global point_stamped, artefact_point_pub for detection in msg.info: - if detection.frame_id == "rgb_camera_optical_link": - object_position = detection.position - class_id = detection.class_id - - # Create a PoseStamped message for the detected object's position - object_pose = tf2_geometry_msgs.PoseStamped() - object_pose.header.frame_id = "rgb_camera_optical_link" - object_pose.pose.position = object_position - object_pose.pose.orientation.w = 1.0 # Don't care about the orientation - - try: - # Transform the object position to the base_link frame - transformed_pose = tf_buffer.transform(object_pose, "base_link", rospy.Duration(1.0)) - transformed_position = transformed_pose.pose.position - rospy.loginfo(f"Detected object (class_id: {class_id}) position in base_link frame: x={transformed_position.x}, y={transformed_position.y}, z={transformed_position.z}") + + object_position = detection.position # in frame of rgb_camera_optic_link + class_id = detection.class_id + + # Create a PoseStamped message for the detected object's position + object_pose = tf2_geometry_msgs.PoseStamped() + object_pose.header.frame_id = "rgb_camera_optical_link" + object_pose.pose.position = object_position + object_pose.pose.orientation.w = 1.0 # Don't care about the orientation + + try: + # Transform the object position to the base_link frame + transformed_pose = tf_buffer.transform(object_pose, "base_link", rospy.Duration(1.0)) + transformed_position = transformed_pose.pose.position - # Publish the transformed detection - transformed_detection = TransformedDetection() - transformed_detection.position = transformed_position - transformed_detection.class_id = class_id - transformed_detection_pub.publish(transformed_detection) + + # Publish the transformed detection + # transformed_detection = TransformedDetection() + # transformed_detection.position = transformed_position + # transformed_detection.class_id = class_id + # transformed_detection_pub.publish(transformed_detection) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - rospy.logerr(f"TF transform error: {e}") + # Publish the position as PointStamped + point_stamped = PointStamped() + point_stamped.header.frame_id = class_id + point_stamped.header.stamp = rospy.Time.now() + point_stamped.point = transformed_position + artefact_point_pub.publish(point_stamped) + + rospy.loginfo(f"Object detected: class_id={point_stamped.header}, pos={point_stamped.point}") + + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + rospy.logerr(f"TF transform error: {e}") def duplicate_rejection(detections): unique_detections = [] @@ -119,13 +137,14 @@ def main(): tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) - + + rospy.Subscriber('/tf', TFMessage, tf_callback) rospy.Subscriber('/tf_static', TFMessage, tf_static_callback) rospy.Subscriber('/object_detector/detection_info', ObjectDetectionInfoArray, detection_callback) - transformed_detection_pub = rospy.Publisher('/transformed_detections', TransformedDetection, queue_size=10) - #rospy.Publisher('/artefact_point', PointStamped) + + rospy.spin() if __name__ == '__main__': From bcb3d05c2d0dc22cb6e3814ddc1721b9e6abbaf2 Mon Sep 17 00:00:00 2001 From: Alexander James Becoy Date: Wed, 3 Jul 2024 12:53:19 +0000 Subject: [PATCH 24/31] Create class for objects, and callbacks --- team6_stack/scripts/object_inspection.py | 44 +++++++++++++++++++++--- 1 file changed, 39 insertions(+), 5 deletions(-) diff --git a/team6_stack/scripts/object_inspection.py b/team6_stack/scripts/object_inspection.py index fbe4fcd..7b60164 100644 --- a/team6_stack/scripts/object_inspection.py +++ b/team6_stack/scripts/object_inspection.py @@ -11,7 +11,7 @@ eps = 1.0 # m, minimum distance between two artefacts min_dist = 2.0 # m, minimum distance from artefact to robot -timer_period = 0.01 # s, timer period +timer_period = 1.0 # s, timer period """ @TODO: - Go to artefact with some distance from it @@ -19,18 +19,43 @@ - How to handle multiple artefacts? - When to remove artefact from the list? - Inspect object from multiple valid angles + +Process: +- Artefact is detected and position in world frame is sent through /artefact_point +- Append list if artefact is new +- If artefact list is not empty, go towards the first artefact in list with some distance + - Distance differs with different object + - Inspect the object from different viable angles (and distance?) +- Remove artefact from list after some moment """ +class Object: + def __init__(self, name, id, min_dist, max_dist): + self.name = name + self.id = id + self.min_dist = min_dist + self.max_dist = max_dist + class ObjectInspectionNode(object): def __init__(self) -> None: rospy.init_node('my_node') rospy.on_shutdown(self.shutdown) + self.objects = [ + Object('backpack', 11, 0.0, 1.0), + Object('umbrella', 24, 0.0, 1.0), + Object('bottle', 25, 0.0, 1.0), + Object('stop_sign', 39, 0.0, 1.0), + Object('clock', 74, 0.0, 1.0), + ] + + self.inspected_artefacts = [] # list of artefacts that are already detected self.artefacts = [] # list of artefacts detected in PointStamped format self.timer = rospy.Timer(rospy.Duration(0, int(timer_period*1e9)), self.timer_callback) self.artefact_sub = rospy.Subscriber('/artefact_point', PointStamped, self.artefact_point_callback) self.waypoint_pub = rospy.Publisher('/way_point', PointStamped, queue_size=1) + # self.inspection_pub = rospy.Publisher('/something something') def run(self) -> None: rospy.spin() @@ -42,7 +67,7 @@ def artefact_point_callback(self, msg: PointStamped) -> None: # Check if the artefact is already in the list for artefact in self.artefacts: if self.distance(artefact, msg) < eps: - rospy.loginfo_throttle(10, "Duplicate artefact detected") + rospy.loginfo_throttle(30, "Duplicate artefact detected") return self.artefacts.append(msg) rospy.loginfo(f"Artefact detected: x={msg.point.x}, y={msg.point.y}, z={msg.point.z}") @@ -62,11 +87,20 @@ def timer_callback(self, event) -> None: rospy.logwarn("Failed to get robot position") return - # Check if the robot is close enough to the artefact + # Get the pose from the artefact to the robot in the world_msf_graph frame artefact = self.artefacts[0] + pose = PoseStamped() + pose.pose.position = artefact.point + pose.header.frame_id = 'world_msf_graph' + pose.header.stamp = rospy.Time() + pose = tf_buffer.transform(pose, 'base_link') + artefact.point = pose.pose.position + + + + # Check if the robot is close enough to the artefact if self.distance(robot_position, artefact) < min_dist: - rospy.loginfo("Robot is close to the artefact") - self.artefacts.pop(0) + rospy.loginfo("Robot is inspecting the artefact") else: # Publish the artefact as a waypoint self.waypoint_pub.publish(artefact) From 3b435d7cdb0347c104fc28a41d1862925779b24f Mon Sep 17 00:00:00 2001 From: YifanZhou99 <75839011+YifanZhou99@users.noreply.github.com> Date: Wed, 3 Jul 2024 13:00:21 +0000 Subject: [PATCH 25/31] in converter change pointstamped to detectioninfo msgtype --- team6_stack/CMakeLists.txt | 2 + .../data/artifacts-20240703-104522.csv | 7 +++ .../{tf_listener.py => tf_converter.py} | 51 +++++++------------ 3 files changed, 28 insertions(+), 32 deletions(-) create mode 100644 team6_stack/data/artifacts-20240703-104522.csv rename team6_stack/scripts/{tf_listener.py => tf_converter.py} (80%) diff --git a/team6_stack/CMakeLists.txt b/team6_stack/CMakeLists.txt index 999be7c..fbe312c 100644 --- a/team6_stack/CMakeLists.txt +++ b/team6_stack/CMakeLists.txt @@ -167,6 +167,8 @@ include_directories( ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS scripts/team6_stack.py + scripts/tf_listener.py + scripts/tf_converter.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/team6_stack/data/artifacts-20240703-104522.csv b/team6_stack/data/artifacts-20240703-104522.csv new file mode 100644 index 0000000..657bf69 --- /dev/null +++ b/team6_stack/data/artifacts-20240703-104522.csv @@ -0,0 +1,7 @@ +class_id,x,y,z +tv,1.8559388954637737,-0.17008074790200362,5.167590858459472 +chair,1.8559388954637737,-0.17008074790200362,5.167590858459472 +stop sign,1.8559388954637737,-0.17008074790200362,5.167590858459472 +chair,1.8679086106366554,-0.1712239208845931,5.2008946706056625 +stop sign,1.8679086106366554,-0.1712239208845931,5.2008946706056625 +tv,1.8679086106366554,-0.1712239208845931,5.2008946706056625 diff --git a/team6_stack/scripts/tf_listener.py b/team6_stack/scripts/tf_converter.py similarity index 80% rename from team6_stack/scripts/tf_listener.py rename to team6_stack/scripts/tf_converter.py index b098ad1..692c368 100644 --- a/team6_stack/scripts/tf_listener.py +++ b/team6_stack/scripts/tf_converter.py @@ -29,7 +29,7 @@ point_stamped = None -artefact_point_pub = rospy.Publisher('/artefact_point', PointStamped, queue_size=10) +detection_info_global_pub = rospy.Publisher('/object_detector/detection_info_global', ObjectDetectionInfoArray, queue_size=10) def tf_callback(msg): @@ -79,36 +79,24 @@ def tf_static_callback(msg): def detection_callback(msg): global point_stamped, artefact_point_pub for detection in msg.info: - - object_position = detection.position # in frame of rgb_camera_optic_link - class_id = detection.class_id - - # Create a PoseStamped message for the detected object's position - object_pose = tf2_geometry_msgs.PoseStamped() - object_pose.header.frame_id = "rgb_camera_optical_link" - object_pose.pose.position = object_position - object_pose.pose.orientation.w = 1.0 # Don't care about the orientation - - try: - # Transform the object position to the base_link frame - transformed_pose = tf_buffer.transform(object_pose, "base_link", rospy.Duration(1.0)) + detection_info_global = detection + + try: + # Transform the camera frame to the world frame + object_pose = tf2_geometry_msgs.PoseStamped() + object_pose.header.frame_id = "rgb_camera_optical_link" + object_pose.pose.position = detection.position # in frame of rgb_camera_optic_link + object_pose.pose.orientation.w = 1.0 # Don't care about the orientation + transformed_pose = tf_buffer.transform(object_pose, "world_graph_msf", rospy.Duration(1.0)) transformed_position = transformed_pose.pose.position - - - # Publish the transformed detection - # transformed_detection = TransformedDetection() - # transformed_detection.position = transformed_position - # transformed_detection.class_id = class_id - # transformed_detection_pub.publish(transformed_detection) - - # Publish the position as PointStamped - point_stamped = PointStamped() - point_stamped.header.frame_id = class_id - point_stamped.header.stamp = rospy.Time.now() - point_stamped.point = transformed_position - artefact_point_pub.publish(point_stamped) - - rospy.loginfo(f"Object detected: class_id={point_stamped.header}, pos={point_stamped.point}") + + detection_info_global.position = transformed_position + + # Publish + detection_info_global_pub.publish(point_stamped) + + # Log + rospy.loginfo(f"Object detected: class_id={point_stamped.header.frame_id}, pos={point_stamped.point}") except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logerr(f"TF transform error: {e}") @@ -131,10 +119,9 @@ def duplicate_rejection(detections): return unique_detections def main(): - global tf_buffer, tf_listener, transformed_detection_pub + global tf_buffer, tf_listener rospy.init_node('tf_listener', anonymous=True) - tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) From 73dc7bdc70164aaa6da126884a2344e0ec4f5096 Mon Sep 17 00:00:00 2001 From: YifanZhou99 <75839011+YifanZhou99@users.noreply.github.com> Date: Wed, 3 Jul 2024 13:19:03 +0000 Subject: [PATCH 26/31] publish /object_detector/detection_info_global --- team6_stack/scripts/tf_converter.py | 23 +++-- team6_stack/scripts/tf_listener.py | 151 ++++++++++++++++++++++++++++ 2 files changed, 163 insertions(+), 11 deletions(-) create mode 100644 team6_stack/scripts/tf_listener.py diff --git a/team6_stack/scripts/tf_converter.py b/team6_stack/scripts/tf_converter.py index 692c368..19bc1a7 100644 --- a/team6_stack/scripts/tf_converter.py +++ b/team6_stack/scripts/tf_converter.py @@ -77,30 +77,31 @@ def tf_static_callback(msg): def detection_callback(msg): - global point_stamped, artefact_point_pub - for detection in msg.info: - detection_info_global = detection + global detection_info_global_pub + detection_info_global = msg + for obj_i, obj_info in enumerate(msg.info): try: # Transform the camera frame to the world frame object_pose = tf2_geometry_msgs.PoseStamped() object_pose.header.frame_id = "rgb_camera_optical_link" - object_pose.pose.position = detection.position # in frame of rgb_camera_optic_link + object_pose.pose.position = obj_info.position # in frame of rgb_camera_optic_link object_pose.pose.orientation.w = 1.0 # Don't care about the orientation transformed_pose = tf_buffer.transform(object_pose, "world_graph_msf", rospy.Duration(1.0)) transformed_position = transformed_pose.pose.position - detection_info_global.position = transformed_position - - # Publish - detection_info_global_pub.publish(point_stamped) - - # Log - rospy.loginfo(f"Object detected: class_id={point_stamped.header.frame_id}, pos={point_stamped.point}") + # Write transformed pos to global msg + detection_info_global.info[obj_i].position = transformed_position except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logerr(f"TF transform error: {e}") + # Publish + detection_info_global_pub.publish(detection_info_global) + + # Log + rospy.loginfo(detection_info_global) + def duplicate_rejection(detections): unique_detections = [] for i, detection in enumerate(detections): diff --git a/team6_stack/scripts/tf_listener.py b/team6_stack/scripts/tf_listener.py new file mode 100644 index 0000000..bce4edc --- /dev/null +++ b/team6_stack/scripts/tf_listener.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python + +import rospy +import tf2_ros +import tf2_msgs +from tf2_msgs.msg import TFMessage +from geometry_msgs.msg import TransformStamped, PointStamped +import tf2_geometry_msgs + +from object_detection_msgs.msg import ObjectDetectionInfoArray + +#from my_transform_package.msg import TransformedDetection + +import numpy as np + +tf_buffer = None +tf_listener = None + +translation_odomGraph2worldGraph = None +rotation_odomGraph2worldGraph = None +translation_base2odomGraph = None +rotation_base2odomGraph = None +translation_world2base = None +rotation_world2base = None +translation_base2cam = None +rotation_base2cam = None +translation_cam2opt = None +rotation_cam2opt = None + + +point_stamped = None +artefact_point_pub = rospy.Publisher('/artefact_point', PointStamped, queue_size=10) + +def tf_callback(msg): + + global translation_odomGraph2worldGraph, rotation_odomGraph2worldGraph + global translation_base2odomGraph, rotation_base2odomGraph + global translation_world2base, rotation_world2base + + for transform in msg.transforms: + # world -> base_link + if transform.header.frame_id == "odom_graph_msf" and transform.child_frame_id == "world_graph_msf": + translation_odomGraph2worldGraph = transform.transform.translation + rotation_odomGraph2worldGraph = transform.transform.rotation + #rospy.loginfo(f"Translation_odomGraph2worldGraph: x={translation_odomGraph2worldGraph.x}, y={translation_odomGraph2worldGraph.y}, z={translation_odomGraph2worldGraph.z}") + #rospy.loginfo(f"Rotation_odomGraph2worldGraph: x={rotation_odomGraph2worldGraph.x}, y={rotation_odomGraph2worldGraph.y}, z={rotation_odomGraph2worldGraph.z}, w={rotation_odomGraph2worldGraph.w} ") + if transform.header.frame_id == "base_link" and transform.child_frame_id == "odom_graph_msf": + translation_base2odomGraph = transform.transform.translation + rotation_base2odomGraph = transform.transform.rotation + #rospy.loginfo(f"Translation_base2odomGraph: x={translation_base2odomGraph.x}, y={translation_base2odomGraph.y}, z={translation_base2odomGraph.z}") + #rospy.loginfo(f"Rotation_base2odomGraph: x={rotation_base2odomGraph.x}, y={rotation_base2odomGraph.y}, z={rotation_base2odomGraph.z}, w={rotation_base2odomGraph.w}") + + if transform.header.frame_id == "odom" and transform.child_frame_id == "base_link": + translation_world2base = transform.transform.translation + rotation_world2base = transform.transform.rotation + #rospy.loginfo(f"Translation_world2base: x={translation_world2base.x}, y={translation_world2base.y}, z={translation_world2base.z}") + #rospy.loginfo(f"Rotation_world2base: x={rotation_world2base.x}, y={rotation_world2base.y}, z={rotation_world2base.z}, w={rotation_world2base.w}") + + +def tf_static_callback(msg): + global translation_base2cam, rotation_base2cam + global translation_cam2opt, rotation_cam2opt + + for transform in msg.transforms: + # base_link -> rgb_camera_link + if transform.header.frame_id == "base_link" and transform.child_frame_id == "rgb_camera_link": + translation_base2cam = transform.transform.translation + rotation_base2cam = transform.transform.rotation + #rospy.loginfo(f"Translation_base2cam: x={translation_base2cam.x}, y={translation_base2cam.y}, z={translation_base2cam.z}") + #rospy.loginfo(f"Rotation_base2cam: x={rotation_base2cam.x}, y={rotation_base2cam.y}, z={rotation_base2cam.z}, w={rotation_base2cam.w}") + # rgb_camera_link -> rgb_camera_optical_link + if transform.header.frame_id == "rgb_camera_link" and transform.child_frame_id == "rgb_camera_optical_link": + translation_cam2opt = transform.transform.translation + rotation_cam2opt = transform.transform.rotation + #rospy.loginfo(f"Translation_cam2opt: x={translation_cam2opt.x}, y={translation_cam2opt.y}, z={translation_cam2opt.z}") + #rospy.loginfo(f"Rotation_cam2opt: x={rotation_cam2opt.x}, y={rotation_cam2opt.y}, z={rotation_cam2opt.z}, w={rotation_cam2opt.w}") + + +def detection_callback(msg): + global point_stamped, artefact_point_pub + for detection in msg.info: + + object_position = detection.position # in frame of rgb_camera_optic_link + class_id = detection.class_id + + # Create a PoseStamped message for the detected object's position + object_pose = tf2_geometry_msgs.PoseStamped() + object_pose.header.frame_id = "rgb_camera_optical_link" + object_pose.pose.position = object_position + object_pose.pose.orientation.w = 1.0 # Don't care about the orientation + + try: + # Transform the object position to the base_link frame + transformed_pose = tf_buffer.transform(object_pose, "base_link", rospy.Duration(1.0)) + transformed_position = transformed_pose.pose.position + + + # Publish the transformed detection + # transformed_detection = TransformedDetection() + # transformed_detection.position = transformed_position + # transformed_detection.class_id = class_id + # transformed_detection_pub.publish(transformed_detection) + + # Publish the position as PointStamped + point_stamped = PointStamped() + point_stamped.header.frame_id = class_id + point_stamped.header.stamp = rospy.Time.now() + point_stamped.point = transformed_position + artefact_point_pub.publish(point_stamped) + + rospy.loginfo(f"Object detected: class_id={point_stamped.header}, pos={point_stamped.point}") + + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + rospy.logerr(f"TF transform error: {e}") + +def duplicate_rejection(detections): + unique_detections = [] + for i, detection in enumerate(detections): + is_duplicate = False + for unique_detection in unique_detections: + dist = np.sqrt( + (detection.position.x - unique_detection.position.x) ** 2 + + (detection.position.y - unique_detection.position.y) ** 2 + + (detection.position.z - unique_detection.position.z) ** 2 + ) + if dist < 0.1: # If objects are closer than 10 cm, consider them duplicates + is_duplicate = True + break + if not is_duplicate: + unique_detections.append(detection) + return unique_detections + +def main(): + global tf_buffer, tf_listener, transformed_detection_pub + + rospy.init_node('tf_listener', anonymous=True) + + tf_buffer = tf2_ros.Buffer() + tf_listener = tf2_ros.TransformListener(tf_buffer) + + + rospy.Subscriber('/tf', TFMessage, tf_callback) + rospy.Subscriber('/tf_static', TFMessage, tf_static_callback) + rospy.Subscriber('/object_detector/detection_info', ObjectDetectionInfoArray, detection_callback) + + + + rospy.spin() + +if __name__ == '__main__': + main() \ No newline at end of file From 5b6cce5518beafbd324a25f5b5145b4de09056de Mon Sep 17 00:00:00 2001 From: Alexander James Becoy Date: Wed, 3 Jul 2024 13:55:56 +0000 Subject: [PATCH 27/31] Create simple object detector node as unique passthrough filter --- team6_stack/CMakeLists.txt | 3 +- team6_stack/package.xml | 2 +- team6_stack/scripts/object_inspection.py | 124 ----------------------- team6_stack/scripts/object_inspector.py | 117 +++++++++++++++++++++ 4 files changed, 120 insertions(+), 126 deletions(-) delete mode 100644 team6_stack/scripts/object_inspection.py create mode 100644 team6_stack/scripts/object_inspector.py diff --git a/team6_stack/CMakeLists.txt b/team6_stack/CMakeLists.txt index 657b0e8..b0ada38 100644 --- a/team6_stack/CMakeLists.txt +++ b/team6_stack/CMakeLists.txt @@ -8,6 +8,7 @@ project(team6_stack) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED) +find_package(object_detection_msgs REQUIRED) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -156,7 +157,7 @@ include_directories( # Mark executable scripts (Python etc.) for installation # in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS - scripts/object_inspection.py + scripts/object_inspector.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/team6_stack/package.xml b/team6_stack/package.xml index b836ea5..306a455 100644 --- a/team6_stack/package.xml +++ b/team6_stack/package.xml @@ -49,7 +49,7 @@ catkin - + object_detection_msgs diff --git a/team6_stack/scripts/object_inspection.py b/team6_stack/scripts/object_inspection.py deleted file mode 100644 index 7b60164..0000000 --- a/team6_stack/scripts/object_inspection.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python - -import rospy -import tf2_ros -import tf2_msgs -from geometry_msgs.msg import TransformStamped -from geometry_msgs.msg import PointStamped, PoseStamped -import tf2_geometry_msgs - -import numpy as np - -eps = 1.0 # m, minimum distance between two artefacts -min_dist = 2.0 # m, minimum distance from artefact to robot -timer_period = 1.0 # s, timer period - -""" @TODO: -- Go to artefact with some distance from it -- Ensure that the waypoint is reachable and not in collision -- How to handle multiple artefacts? -- When to remove artefact from the list? - - Inspect object from multiple valid angles - -Process: -- Artefact is detected and position in world frame is sent through /artefact_point -- Append list if artefact is new -- If artefact list is not empty, go towards the first artefact in list with some distance - - Distance differs with different object - - Inspect the object from different viable angles (and distance?) -- Remove artefact from list after some moment -""" - -class Object: - def __init__(self, name, id, min_dist, max_dist): - self.name = name - self.id = id - self.min_dist = min_dist - self.max_dist = max_dist - -class ObjectInspectionNode(object): - def __init__(self) -> None: - rospy.init_node('my_node') - rospy.on_shutdown(self.shutdown) - - self.objects = [ - Object('backpack', 11, 0.0, 1.0), - Object('umbrella', 24, 0.0, 1.0), - Object('bottle', 25, 0.0, 1.0), - Object('stop_sign', 39, 0.0, 1.0), - Object('clock', 74, 0.0, 1.0), - ] - - self.inspected_artefacts = [] # list of artefacts that are already detected - self.artefacts = [] # list of artefacts detected in PointStamped format - - self.timer = rospy.Timer(rospy.Duration(0, int(timer_period*1e9)), self.timer_callback) - self.artefact_sub = rospy.Subscriber('/artefact_point', PointStamped, self.artefact_point_callback) - self.waypoint_pub = rospy.Publisher('/way_point', PointStamped, queue_size=1) - # self.inspection_pub = rospy.Publisher('/something something') - - def run(self) -> None: - rospy.spin() - - def shutdown(self) -> None: - rospy.loginfo("Shutting down...") - - def artefact_point_callback(self, msg: PointStamped) -> None: - # Check if the artefact is already in the list - for artefact in self.artefacts: - if self.distance(artefact, msg) < eps: - rospy.loginfo_throttle(30, "Duplicate artefact detected") - return - self.artefacts.append(msg) - rospy.loginfo(f"Artefact detected: x={msg.point.x}, y={msg.point.y}, z={msg.point.z}") - - def timer_callback(self, event) -> None: - # Check if there are any artefacts - if not self.artefacts: - return - - # Get the current robot position - tf_buffer = tf2_ros.Buffer() - tf_listener = tf2_ros.TransformListener(tf_buffer) - try: - transform = tf_buffer.lookup_transform('base_link', 'world_msf_graph', rospy.Time()) - robot_position = transform.transform.translation - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): - rospy.logwarn("Failed to get robot position") - return - - # Get the pose from the artefact to the robot in the world_msf_graph frame - artefact = self.artefacts[0] - pose = PoseStamped() - pose.pose.position = artefact.point - pose.header.frame_id = 'world_msf_graph' - pose.header.stamp = rospy.Time() - pose = tf_buffer.transform(pose, 'base_link') - artefact.point = pose.pose.position - - - - # Check if the robot is close enough to the artefact - if self.distance(robot_position, artefact) < min_dist: - rospy.loginfo("Robot is inspecting the artefact") - else: - # Publish the artefact as a waypoint - self.waypoint_pub.publish(artefact) - - def distance(self, p1: PointStamped, p2: PointStamped) -> float: - return np.sqrt( - (p1.point.x - p2.point.x)**2 + \ - (p1.point.y - p2.point.y)**2 + \ - (p1.point.z - p2.point.z)**2 - ) - -def main() -> None: - try: - node = ObjectInspectionNode() - node.run() - except rospy.ROSInterruptException: - pass - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/team6_stack/scripts/object_inspector.py b/team6_stack/scripts/object_inspector.py new file mode 100644 index 0000000..990338a --- /dev/null +++ b/team6_stack/scripts/object_inspector.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python + +import rospy +import tf2_ros +import tf2_msgs +from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import PointStamped, PoseStamped +import tf2_geometry_msgs +from object_detection_msgs.msg import ObjectDetectionInfoArray + +import numpy as np + +""" @TODO: +- Go to artefact with some distance from it +- Ensure that the waypoint is reachable and not in collision +- How to handle multiple artefacts? +- When to remove artefact from the list? + - Inspect object from multiple valid angles + +Process: +- Artefact is detected and position in world frame is sent through /artefact_point +- Append list if artefact is new +- If artefact list is not empty, go towards the first artefact in list with some distance + - Distance differs with different object + - Inspect the object from different viable angles (and distance?) +- Remove artefact from list after nice inspection, and append to inspected list +""" + +class Object: + def __init__(self, name, id, min_dist, max_dist): + self.name = name + self.id = id + self.min_dist = min_dist + self.max_dist = max_dist + +EPS_ = 0.15 # m, minimum distance between two artefacts +TIMER_PERIOD_ = 1.0 # s, timer period + +OBJECTS_ = { + 'backpack' : Object('backpack', 11, 0.0, 1.0), + 'umbrella' : Object('umbrella', 24, 0.0, 1.0), + 'bottle' : Object('bottle', 25, 0.0, 1.0), + 'stop_sign' : Object('stop_sign', 39, 0.0, 1.0), + 'clock' : Object('clock', 74, 0.0, 1.0), +} +MAX_NO_OBJECTS_ = 10 + +class ObjectInspectorNode(object): + def __init__(self) -> None: + rospy.init_node('my_node') + rospy.on_shutdown(self.shutdown) + + self.inspected_artefacts = [] # list of artefacts that are already detected + self.artefacts = [] # list of artefacts detected in PointStamped format + self.is_inspecting = False # + + self.timer = rospy.Timer(rospy.Duration(0, int(TIMER_PERIOD_ * 1e9)), self.timer_callback) + self.detected_artefacts_sub = rospy.Subscriber('/object_detector/detection_info_global', ObjectDetectionInfoArray, self.detected_artefacts_callback) + self.inspected_artefacts_pub = rospy.Publisher('/object_inspector/detection_info_global', ObjectDetectionInfoArray, queue_size=1) + # self.waypoint_pub = rospy.Publisher('/way_point', PointStamped, queue_size=1) + + rospy.loginfo_once('Object Inspector node is running!') + + def run(self) -> None: + rospy.spin() + + def shutdown(self) -> None: + rospy.loginfo("Shutting down...") + + def detected_artefacts_callback(self, msg: ObjectDetectionInfoArray) -> None: + for detected_artefact in msg.info: + already_added = False + + # Check if the artefact is already in the list, or is already inspected + for artefact in self.artefacts + self.inspected_artefacts: + if self.distance(artefact, msg) < EPS_: + already_added = True + break + if already_added: + continue + + self.artefacts.append(detected_artefact) + + def timer_callback(self, event) -> None: + # Check if there are any artefacts + if not self.artefacts: + return + + # Publish detected artefacts + self.publish_inspected_artefacts() + + def publish_inspected_artefacts(self) -> None: + inspected_artefacts_msg = ObjectDetectionInfoArray() + for artefact in self.artefacts: + inspected_artefacts_msg.info.append(artefact) + self.inspected_artefacts.append(artefact) + self.inspected_artefacts_pub.publish(inspected_artefacts_msg) + rospy.loginfo('Inspected artefacts published!') + + + def distance(self, p1: PointStamped, p2: PointStamped) -> float: + return np.sqrt( + (p1.point.x - p2.point.x)**2 + \ + (p1.point.y - p2.point.y)**2 + \ + (p1.point.z - p2.point.z)**2 + ) + +def main() -> None: + try: + node = ObjectInspectorNode() + node.run() + except rospy.ROSInterruptException: + pass + + +if __name__ == '__main__': + main() \ No newline at end of file From 7a694d6ee0bdb1e74d69e7f5e0544fe33ec02efe Mon Sep 17 00:00:00 2001 From: Alexander James Becoy Date: Wed, 3 Jul 2024 14:27:35 +0000 Subject: [PATCH 28/31] Fix minor mistakes --- team6_stack/scripts/object_inspector.py | 29 ++++++++++++++----------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/team6_stack/scripts/object_inspector.py b/team6_stack/scripts/object_inspector.py index 990338a..6214a7b 100644 --- a/team6_stack/scripts/object_inspector.py +++ b/team6_stack/scripts/object_inspector.py @@ -7,6 +7,7 @@ from geometry_msgs.msg import PointStamped, PoseStamped import tf2_geometry_msgs from object_detection_msgs.msg import ObjectDetectionInfoArray +from std_msgs.msg import Bool import numpy as np @@ -34,7 +35,6 @@ def __init__(self, name, id, min_dist, max_dist): self.max_dist = max_dist EPS_ = 0.15 # m, minimum distance between two artefacts -TIMER_PERIOD_ = 1.0 # s, timer period OBJECTS_ = { 'backpack' : Object('backpack', 11, 0.0, 1.0), @@ -51,12 +51,12 @@ def __init__(self) -> None: rospy.on_shutdown(self.shutdown) self.inspected_artefacts = [] # list of artefacts that are already detected - self.artefacts = [] # list of artefacts detected in PointStamped format + self.artefacts = [] # list of artefacts detected self.is_inspecting = False # - self.timer = rospy.Timer(rospy.Duration(0, int(TIMER_PERIOD_ * 1e9)), self.timer_callback) + self.exploration_finish_sub = rospy.Subscriber('exploration_finish', Bool, self.exploration_finish_callback) self.detected_artefacts_sub = rospy.Subscriber('/object_detector/detection_info_global', ObjectDetectionInfoArray, self.detected_artefacts_callback) - self.inspected_artefacts_pub = rospy.Publisher('/object_inspector/detection_info_global', ObjectDetectionInfoArray, queue_size=1) + self.inspected_artefacts_pub = rospy.Publisher('/object_inspector/unique_artifacts', ObjectDetectionInfoArray, queue_size=1) # self.waypoint_pub = rospy.Publisher('/way_point', PointStamped, queue_size=1) rospy.loginfo_once('Object Inspector node is running!') @@ -67,27 +67,30 @@ def run(self) -> None: def shutdown(self) -> None: rospy.loginfo("Shutting down...") + def exploration_finish_callback(self, msg) -> None: + if not msg.data: + return + + # If there are no artefacts detected, do nothing + if len(self.artefacts) == 0: + rospy.logwarn('No artefacts detected!') + + # Publish detected artefacts + self.publish_inspected_artefacts() + def detected_artefacts_callback(self, msg: ObjectDetectionInfoArray) -> None: for detected_artefact in msg.info: already_added = False # Check if the artefact is already in the list, or is already inspected for artefact in self.artefacts + self.inspected_artefacts: - if self.distance(artefact, msg) < EPS_: + if self.distance(detected_artefact.position, artefact.position) < EPS_: already_added = True break if already_added: continue self.artefacts.append(detected_artefact) - - def timer_callback(self, event) -> None: - # Check if there are any artefacts - if not self.artefacts: - return - - # Publish detected artefacts - self.publish_inspected_artefacts() def publish_inspected_artefacts(self) -> None: inspected_artefacts_msg = ObjectDetectionInfoArray() From 27202458f30b6f06ec48c7ba34fedbfb1cc75308 Mon Sep 17 00:00:00 2001 From: Magdalena Yordanova Date: Wed, 3 Jul 2024 14:45:07 +0000 Subject: [PATCH 29/31] adjust subscriber for object_detection_saver --- team6_stack/CMakeLists.txt | 2 +- .../{team6_stack.py => object_detection_saver.py} | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) rename team6_stack/scripts/{team6_stack.py => object_detection_saver.py} (82%) mode change 100755 => 100644 diff --git a/team6_stack/CMakeLists.txt b/team6_stack/CMakeLists.txt index fbe312c..e1ae7ae 100644 --- a/team6_stack/CMakeLists.txt +++ b/team6_stack/CMakeLists.txt @@ -166,7 +166,7 @@ include_directories( ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS - scripts/team6_stack.py + scripts/object_detection_saver.py scripts/tf_listener.py scripts/tf_converter.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/team6_stack/scripts/team6_stack.py b/team6_stack/scripts/object_detection_saver.py old mode 100755 new mode 100644 similarity index 82% rename from team6_stack/scripts/team6_stack.py rename to team6_stack/scripts/object_detection_saver.py index 438fdaa..2629b95 --- a/team6_stack/scripts/team6_stack.py +++ b/team6_stack/scripts/object_detection_saver.py @@ -15,6 +15,7 @@ def callback(msg): for info in msg.info: info = { 'class_id': info.class_id, + # TODO: Add 'id' if it's not always 0 'x': info.position.x, # in camera frame 'y': info.position.y, # in camera frame 'z': info.position.z # in camera frame @@ -38,8 +39,8 @@ def callback(msg): for info in infos: writer.writerow([info['class_id'], info['x'], info['y'], info['z']]) -def object_detection_listener(): - rospy.init_node('team6_stack', anonymous=True) +def object_detection_saver(): + rospy.init_node('object_detection_saver', anonymous=True) global timestr @@ -57,11 +58,12 @@ def object_detection_listener(): writer = csv.writer(file) writer.writerow(['class_id', 'x', 'y', 'z']) - # Subscribe to the /object_detector/detection_info topic - rospy.Subscriber('/object_detector/detection_info', ObjectDetectionInfoArray, callback) + # Subscribe to topics + #rospy.Subscriber('/object_detector/detection_info', ObjectDetectionInfoArray, callback) + rospy.Subscriber('/object_inspector/unique_artifacts', ObjectDetectionInfoArray, callback) # Keep the script running rospy.spin() if __name__ == '__main__': - object_detection_listener() + object_detection_saver() From e67c3b2f936fc13fcfc1010d9411d0c8d75738d8 Mon Sep 17 00:00:00 2001 From: Alexander James Becoy Date: Wed, 3 Jul 2024 14:47:21 +0000 Subject: [PATCH 30/31] Change topic name to reached_home, and remove already inspected artifacts list --- team6_stack/scripts/object_inspector.py | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/team6_stack/scripts/object_inspector.py b/team6_stack/scripts/object_inspector.py index 6214a7b..fe6f8d0 100644 --- a/team6_stack/scripts/object_inspector.py +++ b/team6_stack/scripts/object_inspector.py @@ -50,14 +50,13 @@ def __init__(self) -> None: rospy.init_node('my_node') rospy.on_shutdown(self.shutdown) - self.inspected_artefacts = [] # list of artefacts that are already detected self.artefacts = [] # list of artefacts detected self.is_inspecting = False # - self.exploration_finish_sub = rospy.Subscriber('exploration_finish', Bool, self.exploration_finish_callback) - self.detected_artefacts_sub = rospy.Subscriber('/object_detector/detection_info_global', ObjectDetectionInfoArray, self.detected_artefacts_callback) - self.inspected_artefacts_pub = rospy.Publisher('/object_inspector/unique_artifacts', ObjectDetectionInfoArray, queue_size=1) - # self.waypoint_pub = rospy.Publisher('/way_point', PointStamped, queue_size=1) + self.home_reached_sub = rospy.Subscriber('reached_home', Bool, self.home_reached_callback) + self.detected_artefacts_sub = rospy.Subscriber('object_detector/detection_info_global', ObjectDetectionInfoArray, self.detected_artefacts_callback) + self.inspected_artefacts_pub = rospy.Publisher('object_inspector/unique_artifacts', ObjectDetectionInfoArray, queue_size=1) + self.waypoint_pub = rospy.Publisher('way_point', PointStamped, queue_size=1) rospy.loginfo_once('Object Inspector node is running!') @@ -67,7 +66,7 @@ def run(self) -> None: def shutdown(self) -> None: rospy.loginfo("Shutting down...") - def exploration_finish_callback(self, msg) -> None: + def home_reached_callback(self, msg) -> None: if not msg.data: return @@ -83,7 +82,7 @@ def detected_artefacts_callback(self, msg: ObjectDetectionInfoArray) -> None: already_added = False # Check if the artefact is already in the list, or is already inspected - for artefact in self.artefacts + self.inspected_artefacts: + for artefact in self.artefacts: if self.distance(detected_artefact.position, artefact.position) < EPS_: already_added = True break @@ -94,9 +93,7 @@ def detected_artefacts_callback(self, msg: ObjectDetectionInfoArray) -> None: def publish_inspected_artefacts(self) -> None: inspected_artefacts_msg = ObjectDetectionInfoArray() - for artefact in self.artefacts: - inspected_artefacts_msg.info.append(artefact) - self.inspected_artefacts.append(artefact) + inspected_artefacts_msg.info = [artefact for artefact in self.artefacts] self.inspected_artefacts_pub.publish(inspected_artefacts_msg) rospy.loginfo('Inspected artefacts published!') From 01b5eaf4fd606a92775796a705a96d36491c3c68 Mon Sep 17 00:00:00 2001 From: Alexander James Becoy Date: Wed, 3 Jul 2024 14:57:00 +0000 Subject: [PATCH 31/31] Create launch file for local team6 nodes --- team6_stack/launch/team6_local.launch | 8 ++++++++ team6_stack/package.xml | 3 ++- 2 files changed, 10 insertions(+), 1 deletion(-) create mode 100644 team6_stack/launch/team6_local.launch diff --git a/team6_stack/launch/team6_local.launch b/team6_stack/launch/team6_local.launch new file mode 100644 index 0000000..0e59aaf --- /dev/null +++ b/team6_stack/launch/team6_local.launch @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/team6_stack/package.xml b/team6_stack/package.xml index bd614e3..40f9e18 100644 --- a/team6_stack/package.xml +++ b/team6_stack/package.xml @@ -49,7 +49,8 @@ catkin - object_detection_msgs object_detection + object_detection_msgs + object_detection