diff --git a/launch/navigation_neo.launch.py b/launch/navigation_neo.launch.py
index eaaa60e..6224e73 100644
--- a/launch/navigation_neo.launch.py
+++ b/launch/navigation_neo.launch.py
@@ -21,7 +21,7 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, GroupAction, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.conditions import IfCondition
+from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node, SetParameter
from launch_ros.descriptions import ParameterFile
@@ -37,12 +37,16 @@ def generate_launch_description():
params_file = LaunchConfiguration('params_file')
use_multi_robots = LaunchConfiguration('use_multi_robots')
use_rviz = LaunchConfiguration('use_rviz')
+ graph_filepath = LaunchConfiguration('graph_filepath')
+ use_route = LaunchConfiguration('use_route')
lifecycle_nodes = ['controller_server',
'planner_server',
'behavior_server',
'bt_navigator',
'waypoint_follower']
+ lifecycle_nodes_with_route = lifecycle_nodes + ['smoother_server',
+ 'route_server']
remappings = [('/tf', 'tf'),
('/tf_static', 'tf_static')]
@@ -81,13 +85,32 @@ def generate_launch_description():
declare_use_multi_robots_cmd = DeclareLaunchArgument(
'use_multi_robots', default_value='False',
description='A flag to remove the remappings')
-
+
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz', default_value='True',
description='Launch RViz for visualization'
)
+
+ declare_graph_filepath_cmd = DeclareLaunchArgument(
+ 'graph_filepath', default_value='',
+ description='Full path to the graph file for route planning'
+ )
+
+ declare_use_route_cmd = DeclareLaunchArgument(
+ 'use_route', default_value='False',
+ description='Launch route-specific nodes like smoother and route server'
+ )
+
+ route_rviz_config = os.path.join(bringup_dir, 'rviz', 'route_server.rviz')
+ single_robot_rviz_config = os.path.join(bringup_dir, 'rviz', 'single_robot.rviz')
+ rviz_config = PythonExpression([
+ '"', route_rviz_config, '" if "',
+ use_route, '".lower() == "true" else "',
+ single_robot_rviz_config, '"'
+ ])
+
load_nodes = GroupAction(
- condition=IfCondition(PythonExpression(['not ', use_multi_robots])),
+ condition=UnlessCondition(use_multi_robots),
actions=[
SetParameter('use_sim_time', use_sim_time),
Node(
@@ -124,6 +147,23 @@ def generate_launch_description():
output='screen',
parameters=[configured_params],
remappings=remappings),
+ Node(
+ package='nav2_smoother',
+ executable='smoother_server',
+ name='smoother_server',
+ output='screen',
+ parameters=[configured_params],
+ remappings=remappings,
+ condition=IfCondition(use_route)),
+ Node(
+ package='nav2_route',
+ executable='route_server',
+ name='route_server',
+ output='screen',
+ parameters=[configured_params,
+ {'graph_filepath': graph_filepath}],
+ remappings=remappings,
+ condition=IfCondition(use_route)),
Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
@@ -131,7 +171,17 @@ def generate_launch_description():
output='screen',
parameters=[{'use_sim_time': use_sim_time},
{'autostart': autostart},
- {'node_names': lifecycle_nodes}]),
+ {'node_names': lifecycle_nodes_with_route}],
+ condition=IfCondition(use_route)),
+ Node(
+ package='nav2_lifecycle_manager',
+ executable='lifecycle_manager',
+ name='lifecycle_manager_navigation',
+ output='screen',
+ parameters=[{'use_sim_time': use_sim_time},
+ {'autostart': autostart},
+ {'node_names': lifecycle_nodes}],
+ condition=UnlessCondition(use_route)),
]
)
@@ -168,6 +218,30 @@ def generate_launch_description():
name='waypoint_follower',
output='screen',
parameters=[configured_params]),
+ Node(
+ package='nav2_smoother',
+ executable='smoother_server',
+ name='smoother_server',
+ output='screen',
+ parameters=[configured_params],
+ condition=IfCondition(use_route)),
+ Node(
+ package='nav2_route',
+ executable='route_server',
+ name='route_server',
+ output='screen',
+ parameters=[configured_params,
+ {'graph_filepath': graph_filepath}],
+ condition=IfCondition(use_route)),
+ Node(
+ package='nav2_lifecycle_manager',
+ executable='lifecycle_manager',
+ name='lifecycle_manager_navigation',
+ output='screen',
+ parameters=[{'use_sim_time': use_sim_time},
+ {'autostart': autostart},
+ {'node_names': lifecycle_nodes_with_route}],
+ condition=IfCondition(use_route)),
Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
@@ -175,11 +249,12 @@ def generate_launch_description():
output='screen',
parameters=[{'use_sim_time': use_sim_time},
{'autostart': autostart},
- {'node_names': lifecycle_nodes}]),
+ {'node_names': lifecycle_nodes}],
+ condition=UnlessCondition(use_route)),
]
)
- # Start RViz if use_rviz is True
+ # Start RViz with the chosen config
start_rviz = IncludeLaunchDescription(
condition=IfCondition(use_rviz),
launch_description_source=PythonLaunchDescriptionSource(
@@ -189,6 +264,7 @@ def generate_launch_description():
'use_namespace': use_multi_robots,
'use_sim_time': use_sim_time,
'rviz_output': 'log',
+ 'rviz_config': rviz_config,
}.items(),
)
# Create the launch description and populate
@@ -201,6 +277,8 @@ def generate_launch_description():
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_use_multi_robots_cmd)
ld.add_action(declare_use_rviz_cmd)
+ ld.add_action(declare_use_route_cmd)
+ ld.add_action(declare_graph_filepath_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(load_nodes)
ld.add_action(load_nodes_multi_robot)
diff --git a/package.xml b/package.xml
index 676a105..d472a36 100644
--- a/package.xml
+++ b/package.xml
@@ -13,6 +13,11 @@
nav2_common
slam_toolbox
+ rclpy
+ geometry_msgs
+ std_msgs
+ nav2_simple_commander
+
ament_cmake
diff --git a/rviz/route_server.rviz b/rviz/route_server.rviz
new file mode 100644
index 0000000..6fa37ba
--- /dev/null
+++ b/rviz/route_server.rviz
@@ -0,0 +1,474 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /RobotModel1
+ - /Map1
+ - /Global Planner1
+ - /Global Planner1/Global Costmap1
+ Splitter Ratio: 0.5833333134651184
+ Tree Height: 1588
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded: ~
+ 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_footprint:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ caster_back_left_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_back_right_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_front_left_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_front_right_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ lidar_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ lidar_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wheel_back_left_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wheel_back_right_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wheel_front_left_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wheel_front_right_link:
+ 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
+ - 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: Flat Squares
+ 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: ""
+ 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: Bumper Hit
+ Position Transformer: ""
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.07999999821186066
+ Style: Spheres
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Best Effort
+ Value: /mobile_base/sensors/bumper_pointcloud
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Binary representation: false
+ Binary threshold: 100
+ 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: true
+ 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: true
+ - Class: rviz_common/Group
+ Displays:
+ - Alpha: 0.30000001192092896
+ Binary representation: false
+ Binary threshold: 100
+ 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: 1
+ Buffer Length: 1
+ Class: rviz_default_plugins/Path
+ Color: 26; 95; 180
+ Enabled: true
+ Head Diameter: 0.019999999552965164
+ Head Length: 0.019999999552965164
+ Length: 0.30000001192092896
+ Line Style: Billboards
+ Line Width: 0.07999999821186066
+ 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
+ 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: true
+ Name: Global Planner
+ - Class: rviz_common/Group
+ Displays:
+ - Alpha: 0.699999988079071
+ Binary representation: false
+ Binary threshold: 100
+ 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
+ Enabled: true
+ Name: Controller
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: MarkerArray
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /waypoints
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: MarkerArray
+ Namespaces:
+ route_graph: true
+ route_graph_ids: true
+ Topic:
+ Depth: 5
+ Durability Policy: Transient Local
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: route_graph
+ 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: nav2_rviz_plugins/GoalTool
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Angle: -1.5700000524520874
+ Class: rviz_default_plugins/TopDownOrtho
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Scale: 85.33368682861328
+ Target Frame:
+ Value: TopDownOrtho (rviz_default_plugins)
+ X: -0.38950440287590027
+ Y: -5.073446750640869
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: true
+ Height: 1862
+ Hide Left Dock: true
+ Hide Right Dock: true
+ Navigation 2:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001f4000006a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000070000006a60000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003380000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000c8000000348fc0100000003fb00000018004e0061007600690067006100740069006f006e00200032030000094100000293000002420000034afb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000c80000006a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 3200
+ X: 0
+ Y: 64