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/record_sensors.sh b/record_sensors.sh index 6681106..c015a91 100755 --- a/record_sensors.sh +++ b/record_sensors.sh @@ -25,4 +25,6 @@ rosbag record --output-name=${outpath}/${now}"_smb" \ /imu \ /rslidar/points \ /tf \ -/tf_static +/tf_static \ +/clock \ +/object_detector/detection_info diff --git a/smb_msf_graph/launch/smb_msf_graph.launch b/smb_msf_graph/launch/smb_msf_graph.launch index 8677771..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/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 @@ + + + + diff --git a/smb_opc/rviz/smb_vis.rviz b/smb_opc/rviz/smb_vis.rviz index df59ac6..8fca1c3 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.158384323120117 + Min Value: -1.0142515897750854 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..df59ac6 --- /dev/null +++ b/smb_opc/rviz/smb_vis_default.rviz @@ -0,0 +1,1101 @@ +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 + Splitter Ratio: 0.5111420750617981 + Tree Height: 770 + - 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: 1.5905910730361938 + Min Value: -1.1293858289718628 + 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 + map_o3d_viz: + Value: true + odom_graph_msf: + Value: true + odom_o3d: + 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: + base_link: + LF_WHEEL: + {} + LH_WHEEL: + {} + RF_WHEEL: + {} + RH_WHEEL: + {} + base_inertia: + {} + imu_link: + imu: + {} + 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: + {} + 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: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /sensor_coverage_planner/tare_visualizer/exploring_subspaces + Name: ExplorationSubspaces + Namespaces: + "": true + 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: true + Name: TarePlanner + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + 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: 46.70221710205078 + 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: 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.7397965788841248 + Target Frame: + Yaw: 3.385552406311035 + 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: 000000ff00000000fd0000000400000000000002d000000542fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d000003a2000000fb0100001cfa000000010100000002fb000000180043006f006e00740072006f006c00500061006e0065006c0100000000ffffffff000000d200fffffffb000000100044006900730070006c0061007900730100000000000001ea0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000003e5000000b70000007300fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f0061006401000004a2000000dd0000007300ffffff00000001000001c3000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a400fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000001600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070f0000005efc0100000002fb0000000800540069006d006501000000000000070f000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004390000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + 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: 0 + Y: 27 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 @@ - + 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/CMakeLists.txt b/team6_stack/CMakeLists.txt new file mode 100644 index 0000000..5dd1a25 --- /dev/null +++ b/team6_stack/CMakeLists.txt @@ -0,0 +1,215 @@ +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 COMPONENTS + rospy + object_detection_msgs + object_detection +) + +## 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( +# 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( +# 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/object_inspector.py + scripts/object_detection_saver.py + scripts/tf_listener.py + scripts/tf_converter.py + 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) \ No newline at end of file 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/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/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/launch/team6_stack.launch b/team6_stack/launch/team6_stack.launch new file mode 100644 index 0000000..009e4d7 --- /dev/null +++ b/team6_stack/launch/team6_stack.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/team6_stack/package.xml b/team6_stack/package.xml new file mode 100644 index 0000000..40f9e18 --- /dev/null +++ b/team6_stack/package.xml @@ -0,0 +1,61 @@ + + + team6_stack + 0.0.0 + The team6_stack package + + + + + robotx + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + object_detection_msgs + object_detection + + + + + + + + \ No newline at end of file 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/msf_graph/param_robosense_rs16.lua b/team6_stack/param/msf_graph/param_robosense_rs16.lua new file mode 100644 index 0000000..2e6c6ce --- /dev/null +++ b/team6_stack/param/msf_graph/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 --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 +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 diff --git a/team6_stack/scripts/object_detection_saver.py b/team6_stack/scripts/object_detection_saver.py new file mode 100644 index 0000000..2629b95 --- /dev/null +++ b/team6_stack/scripts/object_detection_saver.py @@ -0,0 +1,69 @@ +#!/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 + +timestr = '' + +def callback(msg): + infos = [] + 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 + } + infos.append(info) + + print(infos) + + # 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_saver(): + rospy.init_node('object_detection_saver', 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 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_saver() diff --git a/team6_stack/scripts/object_inspector.py b/team6_stack/scripts/object_inspector.py new file mode 100644 index 0000000..fe6f8d0 --- /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 +from std_msgs.msg import Bool + +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 + +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.artefacts = [] # list of artefacts detected + self.is_inspecting = False # + + 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!') + + def run(self) -> None: + rospy.spin() + + def shutdown(self) -> None: + rospy.loginfo("Shutting down...") + + def home_reached_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: + if self.distance(detected_artefact.position, artefact.position) < EPS_: + already_added = True + break + if already_added: + continue + + self.artefacts.append(detected_artefact) + + def publish_inspected_artefacts(self) -> None: + inspected_artefacts_msg = ObjectDetectionInfoArray() + inspected_artefacts_msg.info = [artefact for artefact in self.artefacts] + 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 diff --git a/team6_stack/scripts/tf_converter.py b/team6_stack/scripts/tf_converter.py new file mode 100644 index 0000000..19bc1a7 --- /dev/null +++ b/team6_stack/scripts/tf_converter.py @@ -0,0 +1,139 @@ +#!/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 +detection_info_global_pub = rospy.Publisher('/object_detector/detection_info_global', ObjectDetectionInfoArray, 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 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 = 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 + + # 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): + 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 + + 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() 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 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