Skip to content

Commit

Permalink
Jazzy sync 2: August 23, 2024 (#4646)
Browse files Browse the repository at this point in the history
* Updated README Table once Jazzy jobs turn over (#4482)

* add new Jazzy matrix

* missing header

* test toolg

* retry

* done!

* trim

* trim

* fix OS[0]

* shutdown services in destructor of `ClearCostmapService` (#4495)

Signed-off-by: GoesM_server <GoesM@buaa.edu.cn>
Co-authored-by: GoesM_server <GoesM@buaa.edu.cn>

* adjusting backup speed to be more reasonable (#4501)

* Adding Costmap filters to system tests and cleaning up old gazebo classic files (#4502)

* removing old files

* removing old refs to gazebo classic

* porting test body

* including in root

* Dock panel (#4458)

* Initial docking panel

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Only one goal status

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Added dock  pose

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Fix size of text

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Update rviz

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Update rviz config

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

---------

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Fix default view (#4504)

* Fix logo in nav2 panel (#4505)

* Fix logo in nav2 panel

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Move icon

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

---------

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Fix world to map coordinate conversion (#4506)

Signed-off-by: HovorunBh <fipogh@gmail.com>

* Update README.md

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Add dock id (#4511)

* Implement dock id

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Update tests

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Update docs

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Fix virtual override error

Signed-off-by: redvinaa <redvinaa@gmail.com>

---------

Signed-off-by: redvinaa <redvinaa@gmail.com>

* fix(nav2_costmap_2d): make obstacle layer not current on enabled toggle (#4507)

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com>

* min_turning_r_ getting param fix (#4510)

* min_turning_r_ getting param fix

Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com>

* Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com>

---------

Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Ivan Radionov <i.a.radionov@gmail.com.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* fixing gz sim launch file by using gz directly (#4514)

* port wait behavior to new gazebo (#4471)

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Completely rewritten spin, backup, and drive on heading tests to remove flakiness (#4515)

* port backup behavior to new gazebo

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* port drive on heading behavior to new gazebo

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* completely rewritten spin test

* lint

* complete flaky test rewrite

---------

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: stevedan <stevedan.o.omodolor@gmail.com>

* Return out of map update if frames mismatch. Signed-off-by Joey Yang (#4517)

Signed-off-by: Joey Yang <joeyyang.ai@gmail.com>

* Fix error_code_id (#4522)

Signed-off-by: redvinaa <redvinaa@gmail.com>

* completely shutdown dyn_params_handler_ (s)  (#4521)

* completely shutdown dny_params_handler_ in nav2_amcl

Signed-off-by: GoesM_server <GoesM@buaa.edu.cn>

* completely shutdown dyn_param_handler_ in controller_server

Signed-off-by: goes <GoesM@buaa.edu.cn>

* compeletly shutdown dyn_params_handler in nav2_costmap_2d

Signed-off-by: goes <GoesM@buaa.edu.cn>

* compeletly shutdown dyn_param_handler in nav2_docking

Signed-off-by: goes <GoesM@buaa.edu.cn>

* completely shutdown dyn_params_handler in nav2_velocity_smoother

Signed-off-by: goes <GoesM@buaa.edu.cn>

* compeletly shutdown dyn_param_handler in waypoint_follower

Signed-off-by: goes <GoesM@buaa.edu.cn>

* typo fixed

Signed-off-by: goes <GoesM@buaa.edu.cn>

* graceful-controller & dwb_controller

Signed-off-by: goes <GoesM@buaa.edu.cn>

* mppi-controller

Signed-off-by: goes <GoesM@buaa.edu.cn>

* navfn_planner & regulated_..controller

Signed-off-by: goes <GoesM@buaa.edu.cn>

* rotation_..controller & samc_planners

Signed-off-by: goes <GoesM@buaa.edu.cn>

* A*planner

Signed-off-by: goes <GoesM@buaa.edu.cn>

* code style

Signed-off-by: goes <GoesM@buaa.edu.cn>

* 1

Signed-off-by: goes <GoesM@buaa.edu.cn>

* fixed

Signed-off-by: goes <GoesM@buaa.edu.cn>

* fix the usage of weak_ptr

Signed-off-by: goes <GoesM@buaa.edu.cn>

* code-style

Signed-off-by: goes <GoesM@buaa.edu.cn>

* weak_ptr released

Signed-off-by: goes <GoesM@buaa.edu.cn>

* code style

Signed-off-by: goes <GoesM@buaa.edu.cn>

* code style

Signed-off-by: goes <GoesM@buaa.edu.cn>

* code style

Signed-off-by: goes <GoesM@buaa.edu.cn>

* code style update

Signed-off-by: goes <GoesM@buaa.edu.cn>

* back

Signed-off-by: goes <GoesM@buaa.edu.cn>

* rebase conflict resovled

Signed-off-by: goes <GoesM@buaa.edu.cn>

* rebase error fixed

Signed-off-by: goes <GoesM@buaa.edu.cn>

* fixed2

Signed-off-by: goes <GoesM@buaa.edu.cn>

* rebase fixed 3

Signed-off-by: goes <GoesM@buaa.edu.cn>

* 33

Signed-off-by: goes <GoesM@buaa.edu.cn>

* shared_ptr into weak_ptr

Signed-off-by: GoesM <goesm@buaa.edu.cn>

* remove adundant node.resest()

Signed-off-by: GoesM <goesm@buaa.edu.cn>

---------

Signed-off-by: GoesM_server <GoesM@buaa.edu.cn>
Signed-off-by: goes <GoesM@buaa.edu.cn>
Signed-off-by: GoesM <goesm@buaa.edu.cn>
Co-authored-by: GoesM_server <GoesM@buaa.edu.cn>

* check nullptr in smoothPlan() (#4544)

* check nullptr in smoothPlan()

Signed-off-by: GoesM <goesm@buaa.edu.cn>

* code-style

Signed-off-by: GoesM <goesm@buaa.edu.cn>

* code-style

Signed-off-by: GoesM <goesm@buaa.edu.cn>

* simple change

Signed-off-by: GoesM <goesm@buaa.edu.cn>

---------

Signed-off-by: GoesM <goesm@buaa.edu.cn>
Co-authored-by: GoesM <goesm@buaa.edu.cn>

* check nullPtr in `computeControl()` (#4548)

Signed-off-by: goes <GoesM@buaa.edu.cn>
Co-authored-by: goes <GoesM@buaa.edu.cn>

* Straight analytic expansions (#4549)

* Add test to verify analytic expansions are straight

Signed-off-by: James Ward <j.ward@sydney.edu.au>

* Use continuous scaling of angle when performing analytic expansion

Only applies to Hybrid A* - behaviour in lattice planner is unchanged

Signed-off-by: James Ward <j.ward@sydney.edu.au>

---------

Signed-off-by: James Ward <j.ward@sydney.edu.au>

* Rviz tool to get cost of costmap cell (#4546)

* Added GetCost srv

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Added Service in costmap_2d

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Added Rviz tool

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Fixed Styling

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Fixed Styles and Linting

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Fixed Linting

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Added Bool use_footprint to srv

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Added unit test for costmap costcell cost service

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Fixed unit test script

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Added theta, Updated unit test, Updated rviz tool service call logic

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Updated requested changes

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

---------

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Switch to new-style static_transform_publisher arguments. (#4563)

These arguments have been the preferred way to use things
since at least Humble.  This avoids warnings when running it for the tests.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>

* Updated slack link (#4565)

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Update README.md (#4589)

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* fix flickering visualization (#4561)

* Fix Flickering visualization

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Refactoring Costmap2DPublisher and Costmap2DROS

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Refactoring costmap_2d_ros.cpp

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Refactoring Costmap2DPublisher and Costmap2DROS

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Update costmap_2d_publisher.cpp

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Change map_vis_z from float to double

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Add comment to  map_vis_z_ parameter

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

---------

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Copy fix-terminate diff from opennav_docking repo (#4598)

* Copy fix-terminate diff from opennav_docking repo

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Lint

Signed-off-by: redvinaa <redvinaa@gmail.com>

---------

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Fix race condition in AMCL for #4537 (#4605)

* Fixed timed_behavior.hpp (#4602)

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Adding new Nav2 loopback simulator (#4614)

* adding Nav2 loopback sim

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* drop performance by half

* lintin

* Add multirobot usecase comment

* fixing copy paste error

* fixing review comments

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Added laser data from map in nav2_loopback_sim (#4617)

* Added laser data from map

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Fixed Linting

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Fixed Linting

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Fixed few requested changes

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Fixed linting

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Requested changes

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Linting

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Added parameters and fixed requested changes

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* linting

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Added scan  using LineIterator

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* LineIterator max_distance or range_max

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* min of max_distance or range_max

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* final updates working correctly

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Fix lint

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* requested changes

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Update nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Making base frame ID for map to base link transform based on base frame ID parameter (#4632)

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Update Smac Planner for rounding to closest bin rather than flooring (#4636)

* adding stamped option for loopbacks im (#4637)

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: GoesM_server <GoesM@buaa.edu.cn>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: HovorunBh <fipogh@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: redvinaa <redvinaa@gmail.com>
Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: Joey Yang <joeyyang.ai@gmail.com>
Signed-off-by: goes <GoesM@buaa.edu.cn>
Signed-off-by: GoesM <goesm@buaa.edu.cn>
Signed-off-by: James Ward <j.ward@sydney.edu.au>
Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>
Signed-off-by: Chris Lalancette <clalancette@gmail.com>
Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>
Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com>
Co-authored-by: GoesM_server <GoesM@buaa.edu.cn>
Co-authored-by: Alberto Tudela <ajtudela@gmail.com>
Co-authored-by: Bohdan <72872431+HovorunBh@users.noreply.github.com>
Co-authored-by: Vince Reda <60265874+redvinaa@users.noreply.github.com>
Co-authored-by: Kemal Bektas <34746077+bektaskemal@users.noreply.github.com>
Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Co-authored-by: Ivan Radionov <45877502+JJRedmond@users.noreply.github.com>
Co-authored-by: Ivan Radionov <i.a.radionov@gmail.com.com>
Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
Co-authored-by: stevedan <stevedan.o.omodolor@gmail.com>
Co-authored-by: Joey Yang <joeyyang.ai@gmail.com>
Co-authored-by: James Ward <j.ward@sydney.edu.au>
Co-authored-by: Jatin Patil <89979346+JatinPatil2003@users.noreply.github.com>
Co-authored-by: Chris Lalancette <clalancette@gmail.com>
Co-authored-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>
  • Loading branch information
17 people authored Aug 24, 2024
1 parent dbc0882 commit 7969626
Show file tree
Hide file tree
Showing 181 changed files with 4,649 additions and 7,705 deletions.
79 changes: 38 additions & 41 deletions README.md

Large diffs are not rendered by default.

7 changes: 4 additions & 3 deletions nav2_amcl/src/amcl_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -312,7 +312,8 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
pose_pub_->on_deactivate();
particle_cloud_pub_->on_deactivate();

// reset dynamic parameter handler
// shutdown and reset dynamic parameter handler
remove_on_set_parameters_callback(dyn_params_handler_.get());
dyn_params_handler_.reset();

// destroy bond connection
Expand Down Expand Up @@ -540,8 +541,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha
global_frame_id_.c_str());
return;
}
if (abs(msg->pose.pose.position.x) > map_->size_x ||
abs(msg->pose.pose.position.y) > map_->size_y)
if (first_map_received_ && (abs(msg->pose.pose.position.x) > map_->size_x ||
abs(msg->pose.pose.position.y) > map_->size_y))
{
RCLCPP_ERROR(
get_logger(), "Received initialpose from message is out of the size of map. Rejecting.");
Expand Down
18 changes: 9 additions & 9 deletions nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -237,15 +237,6 @@ class TimedBehavior : public nav2_core::Behavior

while (rclcpp::ok()) {
elasped_time_ = clock_->now() - start_time;
if (action_server_->is_cancel_requested()) {
RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str());
stopRobot();
result->total_elapsed_time = elasped_time_;
onActionCompletion(result);
action_server_->terminate_all(result);
return;
}

// TODO(orduno) #868 Enable preempting a Behavior on-the-fly without stopping
if (action_server_->is_preempt_requested()) {
RCLCPP_ERROR(
Expand All @@ -259,6 +250,15 @@ class TimedBehavior : public nav2_core::Behavior
return;
}

if (action_server_->is_cancel_requested()) {
RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str());
stopRobot();
result->total_elapsed_time = elasped_time_;
onActionCompletion(result);
action_server_->terminate_all(result);
return;
}

ResultStatus on_cycle_update_result = onCycleUpdate();
switch (on_cycle_update_result.status) {
case Status::SUCCEEDED:
Expand Down
11 changes: 9 additions & 2 deletions nav2_bringup/launch/bringup_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@ def generate_launch_description():
use_composition = LaunchConfiguration('use_composition')
use_respawn = LaunchConfiguration('use_respawn')
log_level = LaunchConfiguration('log_level')
use_localization = LaunchConfiguration('use_localization')

# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
Expand Down Expand Up @@ -99,6 +100,11 @@ def generate_launch_description():
'map', default_value='', description='Full path to map yaml file to load'
)

declare_use_localization_cmd = DeclareLaunchArgument(
'use_localization', default_value='True',
description='Whether to enable localization or not'
)

declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
Expand Down Expand Up @@ -151,7 +157,7 @@ def generate_launch_description():
PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'slam_launch.py')
),
condition=IfCondition(slam),
condition=IfCondition(PythonExpression([slam, ' and ', use_localization])),
launch_arguments={
'namespace': namespace,
'use_sim_time': use_sim_time,
Expand All @@ -164,7 +170,7 @@ def generate_launch_description():
PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'localization_launch.py')
),
condition=IfCondition(PythonExpression(['not ', slam])),
condition=IfCondition(PythonExpression(['not ', slam, ' and ', use_localization])),
launch_arguments={
'namespace': namespace,
'map': map_yaml_file,
Expand Down Expand Up @@ -210,6 +216,7 @@ def generate_launch_description():
ld.add_action(declare_use_composition_cmd)
ld.add_action(declare_use_respawn_cmd)
ld.add_action(declare_log_level_cmd)
ld.add_action(declare_use_localization_cmd)

# Add the actions to launch all of the navigation nodes
ld.add_action(bringup_cmd_group)
Expand Down
9 changes: 4 additions & 5 deletions nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -111,11 +111,10 @@ def generate_launch_description():
world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
world_sdf_xacro = ExecuteProcess(
cmd=['xacro', '-o', world_sdf, ['headless:=', 'False'], world])
start_gazebo_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('ros_gz_sim'), 'launch',
'gz_sim.launch.py')),
launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items())
start_gazebo_cmd = ExecuteProcess(
cmd=['gz', 'sim', '-r', '-s', world_sdf],
output='screen',
)

remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown(
on_shutdown=[
Expand Down
224 changes: 224 additions & 0 deletions nav2_bringup/launch/tb3_loopback_simulation.launch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,224 @@
# Copyright (C) 2024 Open Navigation LLC
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""This is all-in-one launch script intended for use by nav2 developers."""

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
GroupAction,
IncludeLaunchDescription,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node, SetParameter
from launch_ros.descriptions import ParameterFile

from nav2_common.launch import RewrittenYaml


def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
loopback_sim_dir = get_package_share_directory('nav2_loopback_sim')
launch_dir = os.path.join(bringup_dir, 'launch')
sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')

# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
use_namespace = LaunchConfiguration('use_namespace')
map_yaml_file = LaunchConfiguration('map')
params_file = LaunchConfiguration('params_file')
autostart = LaunchConfiguration('autostart')
use_composition = LaunchConfiguration('use_composition')
use_respawn = LaunchConfiguration('use_respawn')

# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
use_rviz = LaunchConfiguration('use_rviz')

remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]

# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
'namespace', default_value='', description='Top-level namespace'
)

declare_use_namespace_cmd = DeclareLaunchArgument(
'use_namespace',
default_value='false',
description='Whether to apply a namespace to the navigation stack',
)

declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'),
)

declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
description='Full path to the ROS2 parameters file to use for all launched nodes',
)

declare_autostart_cmd = DeclareLaunchArgument(
'autostart',
default_value='true',
description='Automatically startup the nav2 stack',
)

declare_use_composition_cmd = DeclareLaunchArgument(
'use_composition',
default_value='True',
description='Whether to use composed bringup',
)

declare_use_respawn_cmd = DeclareLaunchArgument(
'use_respawn',
default_value='False',
description='Whether to respawn if a node crashes. Applied when composition is disabled.',
)

declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config_file',
default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'),
description='Full path to the RVIZ config file to use',
)

declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
'use_robot_state_pub',
default_value='True',
description='Whether to start the robot state publisher',
)

declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz', default_value='True', description='Whether to start RVIZ'
)

urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
with open(urdf, 'r') as infp:
robot_description = infp.read()

start_robot_state_publisher_cmd = Node(
condition=IfCondition(use_robot_state_pub),
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
namespace=namespace,
output='screen',
parameters=[
{'use_sim_time': True, 'robot_description': robot_description}
],
remappings=remappings,
)

rviz_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')),
condition=IfCondition(use_rviz),
launch_arguments={
'namespace': namespace,
'use_namespace': use_namespace,
'use_sim_time': 'True',
'rviz_config': rviz_config_file,
}.items(),
)

bringup_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')),
launch_arguments={
'namespace': namespace,
'use_namespace': use_namespace,
'map': map_yaml_file,
'use_sim_time': 'True',
'params_file': params_file,
'autostart': autostart,
'use_composition': use_composition,
'use_respawn': use_respawn,
'use_localization': 'False', # Don't use SLAM, AMCL
}.items(),
)

loopback_sim_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(loopback_sim_dir, 'loopback_simulation.launch.py')),
launch_arguments={
'params_file': params_file,
}.items(),
)

configured_params = ParameterFile(
RewrittenYaml(
source_file=params_file,
root_key=namespace,
param_rewrites={},
convert_types=True,
),
allow_substs=True,
)

start_map_server = GroupAction(
actions=[
SetParameter('use_sim_time', True),
Node(
package='nav2_map_server',
executable='map_server',
name='map_server',
output='screen',
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params, {'yaml_filename': map_yaml_file}],
remappings=remappings,
),
Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_map_server',
output='screen',
parameters=[
configured_params,
{'autostart': autostart}, {'node_names': ['map_server']}],
),
]
)

# Create the launch description and populate
ld = LaunchDescription()

# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_use_composition_cmd)

ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(declare_use_respawn_cmd)

# Add the actions to launch all of the navigation nodes
ld.add_action(start_robot_state_publisher_cmd)
ld.add_action(start_map_server)
ld.add_action(loopback_sim_cmd)
ld.add_action(rviz_cmd)
ld.add_action(bringup_cmd)

return ld
11 changes: 5 additions & 6 deletions nav2_bringup/launch/tb3_simulation_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -214,12 +214,11 @@ def generate_launch_description():
world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
world_sdf_xacro = ExecuteProcess(
cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world])
gazebo_server = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('ros_gz_sim'), 'launch',
'gz_sim.launch.py')),
launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items(),
condition=IfCondition(use_simulator))
gazebo_server = ExecuteProcess(
cmd=['gz', 'sim', '-r', '-s', world_sdf],
output='screen',
condition=IfCondition(use_simulator)
)

remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown(
on_shutdown=[
Expand Down
Loading

0 comments on commit 7969626

Please sign in to comment.