diff --git a/nav.sh b/nav.sh index 10395dc..6ae52a4 100644 --- a/nav.sh +++ b/nav.sh @@ -12,8 +12,9 @@ commands=( "ros2 launch rm_simpal_move simple_move.launch.py" "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py" # "/usr/bin/python3 /home/robofish/RC2025/src/rc_lidar/juxing.py" - "/usr/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py" - # "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map" + "/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py" + "/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py" + "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py" ) diff --git a/nav1.sh b/nav1.sh index 2bd46f4..5da90df 100644 --- a/nav1.sh +++ b/nav1.sh @@ -11,7 +11,9 @@ commands=( nav_rviz:=true" "ros2 launch rm_simpal_move simple_move.launch.py" "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py" - "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map1" + "/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py" + "/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py" + "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py" ) diff --git a/nav2.sh b/nav2.sh index fcfc471..817be1f 100644 --- a/nav2.sh +++ b/nav2.sh @@ -6,12 +6,14 @@ commands=( world:=map2 \ mode:=nav \ lio:=fastlio \ - localization:=amcl \ + localization:=icp \ lio_rviz:=false \ nav_rviz:=true" "ros2 launch rm_simpal_move simple_move.launch.py" "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py" - "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map2" + "/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py" + "/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py" + "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py" ) diff --git a/nav21.rviz b/nav21.rviz new file mode 100644 index 0000000..2e0a441 --- /dev/null +++ b/nav21.rviz @@ -0,0 +1,677 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Tree1 + - /Map2 + Splitter Ratio: 0.5833333134651184 + Tree Height: 462 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + livox_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + base_link_fake: + Value: true + goal_pose: + Value: true + lidar_odom: + Value: true + livox_frame: + Value: true + map: + Value: true + odom: + Value: true + wheel_1: + Value: true + wheel_2: + Value: true + wheel_3: + Value: true + wheel_4: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + goal_pose: + {} + odom: + lidar_odom: + base_link: + base_link_fake: + {} + livox_frame: + {} + wheel_1: + {} + wheel_2: + {} + wheel_3: + {} + wheel_4: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: voxel_grid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_grid + 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_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.6000000238418579 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: false + Max Arrow Length: 0.30000001192092896 + Min Arrow Length: 0.019999999552965164 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: false + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: false + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Controller + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: false + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /teb_poses + Value: false + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /teb_markers + Value: false + - Class: rviz_default_plugins/Axes + Enabled: false + Length: 0.5 + Name: base_link_fake + Radius: 0.07999999821186066 + Reference Frame: base_link_fake + Value: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar_grid_updates + Use Timestamp: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.10038423538208 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.5876622200012207 + Y: 1.5225260257720947 + Z: 0.4079643487930298 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 5.280543804168701 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 932 + Hide Left Dock: false + Hide Right Dock: false + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000438000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000020b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032010000024e000001390000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004ad0000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1545 + X: 375 + Y: 74 diff --git a/src/rc_lidar.zip b/src/rc_lidar.zip new file mode 100644 index 0000000..1351eae Binary files /dev/null and b/src/rc_lidar.zip differ diff --git a/src/rc_lidar/caijian.py b/src/rc_lidar/caijian.py index 17d8ab4..6511038 100644 --- a/src/rc_lidar/caijian.py +++ b/src/rc_lidar/caijian.py @@ -31,7 +31,7 @@ class LidarFilterNode(Node): points[i] = [x, y, z, intensity] z_mask = (points[:,2] >= 1.5) & (points[:,2] <= 3.0) - dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 12.0 + dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 16.0 mask = z_mask & dist_mask filtered_points = points[mask] diff --git a/src/rc_lidar/zhaoban.py b/src/rc_lidar/zhaoban.py index 556027e..6268cf2 100644 --- a/src/rc_lidar/zhaoban.py +++ b/src/rc_lidar/zhaoban.py @@ -19,7 +19,7 @@ def find_circle(points, eps=0.5, min_samples=10): # 聚类 clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(points) labels = clustering.labels_ - # 只取最大簇 + # 只取最大簇 unique, counts = np.unique(labels[labels != -1], return_counts=True) if len(unique) == 0: raise ValueError("未找到有效聚类") diff --git a/src/rm_nav_bringup/config/reality/measurement_params_real.yaml b/src/rm_nav_bringup/config/reality/measurement_params_real.yaml index 41fe73e..4586ff8 100644 --- a/src/rm_nav_bringup/config/reality/measurement_params_real.yaml +++ b/src/rm_nav_bringup/config/reality/measurement_params_real.yaml @@ -1,3 +1,3 @@ base_link2livox_frame: - xyz: "\"0.01906 -0.33 0.41\"" + xyz: "\"0.00906 -0.33 0.41\"" rpy: "\"0.0 0.0 0.0\"" \ No newline at end of file