diff --git a/toid_navigation/launch/main.py b/toid_navigation/launch/main.py index 9c02541..92ee94c 100644 --- 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, diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index aa399fc..d08280b 100644 --- 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 diff --git a/toid_navigation/rviz/nav2.rviz b/toid_navigation/rviz/nav2.rviz index 5eaf224..981ff64 100644 --- 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: 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