Added rotate acorns

2026-04-15 13:33:50 +02:00
parent 2619a9b0b5
commit b683dc0bb4
3 changed files with 89 additions and 18 deletions
--- a/toid_navigation/launch/main.py
+++ b/toid_navigation/launch/main.py
@@ -1,7 +1,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.conditions import IfCondition, UnlessCondition 
-from launch.substitutions import  LaunchConfiguration
+from launch.substitutions import  LaunchConfiguration, AllSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -10,6 +10,7 @@ import os
 def generate_launch_description():
    nav_pkg_share = FindPackageShare("").find('toid_navigation')
    control_pkg_share = FindPackageShare("").find('toid_control')
+    lidar_pkg_share = FindPackageShare("").find('toid_lidar')
    params = os.path.join(nav_pkg_share, 'params', 'toid_general_params.yaml')
    map = os.path.join(nav_pkg_share, 'maps', 'mapb2_5cm.yaml')
    ctrl_launch_dir = os.path.join(control_pkg_share, 'launch')
@@ -37,6 +38,20 @@ def generate_launch_description():
        default_value='True',
        description="Whether to launch rviz2" 
    )
+
+    use_lidar = LaunchConfiguration("use_lidar")
+    use_lidar_arg = DeclareLaunchArgument(
+        'use_lidar',
+        default_value='True',
+        description="Whether to launch rviz2" 
+    )
+
+    is_blue = LaunchConfiguration("is_blue")
+    is_blue_arg = DeclareLaunchArgument(
+        'is_blue',
+        default_value='True',
+        description="Whether to launch rviz2" 
+    )
    
    toid_control = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
@@ -49,6 +64,17 @@ def generate_launch_description():
        condition=IfCondition(run_nodes),
    )

+    toid_lidar = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_pkg_share, 'launch' , 'launch.py')
+        ),
+        launch_arguments={
+            'visualize': 'False',
+            'lidar_frame': 'lidar_frame', 
+        }.items(),
+        condition=IfCondition(AllSubstitution(run_nodes, use_lidar)),
+    ) 
+
    map_server = Node(
        package='nav2_map_server',
        executable='map_server',
@@ -118,6 +144,8 @@ def generate_launch_description():
        visualize_arg,
        use_mock_arg,
        run_nodes_arg,
+        use_lidar_arg,
+        toid_lidar,
        rviz_node,
        map_server,
        bt_navigator,
--- a/toid_navigation/params/toid_general_params.yaml
+++ b/toid_navigation/params/toid_general_params.yaml
@@ -22,7 +22,7 @@ behavior_server:
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 50.0
-    behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords", "approachAcorns"]
+    behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords", "approachAcorns", rotateAcorns]
    spin:
      plugin: "nav2_behaviors::Spin"
    backup:
@@ -41,6 +41,8 @@ behavior_server:
      kdelta: 1.0
      lambda: 2.0
      debug_marker: true
+    rotateAcorns:
+      plugin: "toid::RotateAcorns"
    local_frame: map
    global_frame: map
    robot_base_frame: base_footprint
@@ -58,7 +60,7 @@ global_costmap:
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_footprint
-      footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]"
+      footprint: "[[-0.045, 0.15], [0.255, 0.15], [0.255, -0.15], [-0.045, -0.15]]"
      footprint_padding: 0.02
      track_unknown_space: false
      rolling_window: false
@@ -79,7 +81,7 @@ local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
-      footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]"
+      footprint: "[[-0.045, 0.15], [0.255, 0.15], [0.255, -0.15], [-0.045, -0.15]]"
      footprint_padding: 0.01
      #robot_radius: 0.18
      global_frame: map
@@ -89,11 +91,14 @@ local_costmap:
      height: 1
      resolution: 0.01
      introspection_mode: "disabled"
-      plugins: ["static_layer", "inflation_layer"]
+      plugins: ["static_layer", rival_layer, "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        footprint_clearing_enabled: true
        map_subscribe_transient_local: True
+      rival_layer:
+        plugin: "toid::RivalLayer"
+        rival_size: 0.15
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
--- a/toid_navigation/rviz/nav2.rviz
+++ b/toid_navigation/rviz/nav2.rviz
@@ -1,13 +1,14 @@
 Panels:
  - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 138
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /RobotModel1
        - /GlobalCostMap1/Topic1
+        - /Pose1
      Splitter Ratio: 0.5
-    Tree Height: 592
+    Tree Height: 668
  - Class: rviz_common/Selection
    Name: Selection
  - Class: rviz_common/Tool Properties
@@ -74,6 +75,10 @@ Visualization Manager:
          Show Axes: false
          Show Trail: false
          Value: true
+        camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
        drivewhl_l_link:
          Alpha: 1
          Show Axes: false
@@ -247,6 +252,39 @@ Visualization Manager:
        Reliability Policy: Reliable
        Value: /start_point
      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Marker
+      Namespaces:
+        "": true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /marker
+      Value: true
+    - Alpha: 1
+      Axes Length: 0.10000000149011612
+      Axes Radius: 0.019999999552965164
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Axes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /closest_acorn
+      Value: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
@@ -293,33 +331,33 @@ Visualization Manager:
  Views:
    Current:
      Class: rviz_default_plugins/XYOrbit
-      Distance: 3.863811731338501
+      Distance: 2.2019217014312744
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
-        X: 0.1778966784477234
-        Y: -1.1747734546661377
-        Z: 1.7285346984863281e-06
+        X: 0.3222081661224365
+        Y: -0.08033189922571182
+        Z: 0
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
-      Pitch: 1.3697962760925293
+      Pitch: 1.064796805381775
      Target Frame: <Fixed Frame>
      Value: XYOrbit (rviz_default_plugins)
-      Yaw: 4.628584861755371
+      Yaw: 4.925206184387207
    Saved: ~
 Window Geometry:
  Displays:
    collapsed: false
-  Height: 896
+  Height: 1186
  Hide Left Dock: false
  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000254000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002de000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002de000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005d30000003efc0100000002fb0000000800540069006d00650100000000000005d30000027b00fffffffb0000000800540069006d0065010000000000000450000000000000000000000264000002de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000025400000398fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000003980000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000003980000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070c0000005efc0100000002fb0000000800540069006d006501000000000000070c0000048700fffffffb0000000800540069006d00650100000000000004500000000000000000000003450000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
@@ -328,6 +366,6 @@ Window Geometry:
    collapsed: false
  Views:
    collapsed: false
-  Width: 1491
-  X: 429
-  Y: 75
+  Width: 1804
+  X: 428
+  Y: 74