From b50e85ba7b6c240897063a586d62b500b5593fb6 Mon Sep 17 00:00:00 2001 From: Pradheep Date: Fri, 12 Sep 2025 13:58:59 +0200 Subject: [PATCH 1/3] 1.0.4 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index a25ab85..2265e78 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ neo_nav2_bringup - 1.0.3 + 1.0.4 ROS-2 navigation bringup packages for neobotix robots Pradheep Padmanabhan Apache-2.0 From 4e3a3e520f1ca0a7013aae67f0f39faad95b9970 Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Fri, 28 Nov 2025 13:21:39 +0530 Subject: [PATCH 2/3] added support for route server Signed-off-by: Adarsh Karan Kesavadas Prasanth --- launch/navigation_neo.launch.py | 90 +++++- launch/rviz_launch.py | 2 +- package.xml | 5 + rviz/route_server.rviz | 474 ++++++++++++++++++++++++++++++++ 4 files changed, 564 insertions(+), 7 deletions(-) create mode 100644 rviz/route_server.rviz 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/launch/rviz_launch.py b/launch/rviz_launch.py index 0f46bf7..8e2aa52 100644 --- a/launch/rviz_launch.py +++ b/launch/rviz_launch.py @@ -99,7 +99,7 @@ def generate_launch_description(): default_value=os.path.join( get_package_share_directory('neo_nav2_bringup'), 'rviz', - 'single_robot.rviz'), + 'route_server.rviz'), description='Full path to the RVIZ config file to use') declare_rviz_output_cmd = DeclareLaunchArgument( diff --git a/package.xml b/package.xml index 2265e78..f436f66 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 From fe86a69ea90be3d31b1cfcfb2153dd6a8108545f Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Fri, 28 Nov 2025 13:32:14 +0530 Subject: [PATCH 3/3] reverted back default value Signed-off-by: Adarsh Karan Kesavadas Prasanth --- launch/rviz_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/rviz_launch.py b/launch/rviz_launch.py index 8e2aa52..0f46bf7 100644 --- a/launch/rviz_launch.py +++ b/launch/rviz_launch.py @@ -99,7 +99,7 @@ def generate_launch_description(): default_value=os.path.join( get_package_share_directory('neo_nav2_bringup'), 'rviz', - 'route_server.rviz'), + 'single_robot.rviz'), description='Full path to the RVIZ config file to use') declare_rviz_output_cmd = DeclareLaunchArgument(