Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Modify folders to make it compatible for ros2 humble #6

Merged
merged 1 commit into from
Mar 14, 2024
Merged

Modify folders to make it compatible for ros2 humble #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion nav2_amcl/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_amcl</name>
<version>1.2.0</version>
<version>1.1.13</version>
<description>
<p>
amcl is a probabilistic localization system for a robot moving in
Expand Down
22 changes: 6 additions & 16 deletions nav2_amcl/src/amcl_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -328,12 +328,10 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
nomotion_update_srv_.reset();
initial_pose_sub_.reset();
laser_scan_connection_.disconnect();
tf_listener_.reset(); // listener may access lase_scan_filter_, so it should be reset earlier
laser_scan_filter_.reset();
laser_scan_sub_.reset();

// Map
map_sub_.reset(); // map_sub_ may access map_, so it should be reset earlier
if (map_ != NULL) {
map_free(map_);
map_ = nullptr;
Expand All @@ -343,6 +341,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)

// Transforms
tf_broadcaster_.reset();
tf_listener_.reset();
tf_buffer_.reset();

// PubSub
Expand Down Expand Up @@ -809,15 +808,6 @@ bool AmclNode::updateFilter(
get_logger(), "Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min,
angle_increment);

// Check the validity of range_max, must > 0.0
if (laser_scan->range_max <= 0.0) {
RCLCPP_WARN(
get_logger(), "wrong range_max of laser_scan data: %f. The message could be malformed."
" Ignore this message and stop updating.",
laser_scan->range_max);
return false;
}

// Apply range min/max thresholds, if the user supplied them
if (laser_max_range_ > 0.0) {
ldata.range_max = std::min(laser_scan->range_max, static_cast<float>(laser_max_range_));
Expand Down Expand Up @@ -1168,7 +1158,7 @@ AmclNode::dynamicParametersCallback(
if (param_type == ParameterType::PARAMETER_DOUBLE) {
if (param_name == "alpha1") {
alpha1_ = parameter.as_double();
// alpha restricted to be non-negative
//alpha restricted to be non-negative
if (alpha1_ < 0.0) {
RCLCPP_WARN(
get_logger(), "You've set alpha1 to be negative,"
Expand All @@ -1178,7 +1168,7 @@ AmclNode::dynamicParametersCallback(
reinit_odom = true;
} else if (param_name == "alpha2") {
alpha2_ = parameter.as_double();
// alpha restricted to be non-negative
//alpha restricted to be non-negative
if (alpha2_ < 0.0) {
RCLCPP_WARN(
get_logger(), "You've set alpha2 to be negative,"
Expand All @@ -1188,7 +1178,7 @@ AmclNode::dynamicParametersCallback(
reinit_odom = true;
} else if (param_name == "alpha3") {
alpha3_ = parameter.as_double();
// alpha restricted to be non-negative
//alpha restricted to be non-negative
if (alpha3_ < 0.0) {
RCLCPP_WARN(
get_logger(), "You've set alpha3 to be negative,"
Expand All @@ -1198,7 +1188,7 @@ AmclNode::dynamicParametersCallback(
reinit_odom = true;
} else if (param_name == "alpha4") {
alpha4_ = parameter.as_double();
// alpha restricted to be non-negative
//alpha restricted to be non-negative
if (alpha4_ < 0.0) {
RCLCPP_WARN(
get_logger(), "You've set alpha4 to be negative,"
Expand All @@ -1208,7 +1198,7 @@ AmclNode::dynamicParametersCallback(
reinit_odom = true;
} else if (param_name == "alpha5") {
alpha5_ = parameter.as_double();
// alpha restricted to be non-negative
//alpha restricted to be non-negative
if (alpha5_ < 0.0) {
RCLCPP_WARN(
get_logger(), "You've set alpha5 to be negative,"
Expand Down
4 changes: 2 additions & 2 deletions nav2_amcl/src/pf/eig3.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ static void tred2(double V[n][n], double d[n], double e[n])
// Fortran subroutine in EISPACK.

int i, j, k;
double f, g, hh;
double f, g, h, hh;
for (j = 0; j < n; j++) {
d[j] = V[n - 1][j];
}
Expand Down Expand Up @@ -122,7 +122,7 @@ static void tred2(double V[n][n], double d[n], double e[n])
for (i = 0; i < n - 1; i++) {
V[n - 1][i] = V[i][i];
V[i][i] = 1.0;
const double h = d[i + 1];
h = d[i + 1];
if (h != 0.0) {
for (k = 0; k <= i; k++) {
d[k] = V[k][i + 1] / h;
Expand Down
184 changes: 82 additions & 102 deletions nav2_bringup/launch/bringup_launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,19 +17,15 @@
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
GroupAction,
IncludeLaunchDescription,
SetEnvironmentVariable,
)
from launch.actions import (DeclareLaunchArgument, GroupAction,
IncludeLaunchDescription, SetEnvironmentVariable)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.actions import PushROSNamespace
from launch_ros.actions import PushRosNamespace
from launch_ros.descriptions import ParameterFile
from nav2_common.launch import ReplaceString, RewrittenYaml
from nav2_common.launch import RewrittenYaml, ReplaceString


def generate_launch_description():
Expand All @@ -55,7 +51,13 @@ def generate_launch_description():
# https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping`
# https://github.com/ros2/launch_ros/issues/56
remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
remappings = [('/tf', 'tf'),
('/tf_static', 'tf_static')]

# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time,
'yaml_filename': map_yaml_file}

# Only it applys when `use_namespace` is True.
# '<robot_namespace>' keyword shall be replaced by 'namespace' launch argument
Expand All @@ -64,134 +66,112 @@ def generate_launch_description():
params_file = ReplaceString(
source_file=params_file,
replacements={'<robot_namespace>': ('/', namespace)},
condition=IfCondition(use_namespace),
)
condition=IfCondition(use_namespace))

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

stdout_linebuf_envvar = SetEnvironmentVariable(
'RCUTILS_LOGGING_BUFFERED_STREAM', '1'
)
'RCUTILS_LOGGING_BUFFERED_STREAM', '1')

declare_namespace_cmd = DeclareLaunchArgument(
'namespace', default_value='', description='Top-level namespace'
)
'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',
)
description='Whether to apply a namespace to the navigation stack')

declare_slam_cmd = DeclareLaunchArgument(
'slam', default_value='False', description='Whether run a SLAM'
)
'slam',
default_value='False',
description='Whether run a SLAM')

declare_map_yaml_cmd = DeclareLaunchArgument(
'map', default_value='', description='Full path to map yaml file to load'
)
'map',
description='Full path to map yaml file to load')

declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true',
)
description='Use simulation (Gazebo) clock if true')

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',
)
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',
)
'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',
)
'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.',
)
'use_respawn', default_value='False',
description='Whether to respawn if a node crashes. Applied when composition is disabled.')

declare_log_level_cmd = DeclareLaunchArgument(
'log_level', default_value='info', description='log level'
)
'log_level', default_value='info',
description='log level')

# Specify the actions
bringup_cmd_group = GroupAction(
[
PushROSNamespace(condition=IfCondition(use_namespace), namespace=namespace),
Node(
condition=IfCondition(use_composition),
name='nav2_container',
package='rclcpp_components',
executable='component_container_isolated',
parameters=[configured_params, {'autostart': autostart}],
arguments=['--ros-args', '--log-level', log_level],
remappings=remappings,
output='screen',
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'slam_launch.py')
),
condition=IfCondition(slam),
launch_arguments={
'namespace': namespace,
'use_sim_time': use_sim_time,
'autostart': autostart,
'use_respawn': use_respawn,
'params_file': params_file,
}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'localization_launch.py')
),
condition=IfCondition(PythonExpression(['not ', slam])),
launch_arguments={
'namespace': namespace,
'map': map_yaml_file,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
'use_composition': use_composition,
'use_respawn': use_respawn,
'container_name': 'nav2_container',
}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'navigation_launch.py')
),
launch_arguments={
'namespace': namespace,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
'use_composition': use_composition,
'use_respawn': use_respawn,
'container_name': 'nav2_container',
}.items(),
),
]
)
bringup_cmd_group = GroupAction([
PushRosNamespace(
condition=IfCondition(use_namespace),
namespace=namespace),

Node(
condition=IfCondition(use_composition),
name='nav2_container',
package='rclcpp_components',
executable='component_container_isolated',
parameters=[configured_params, {'autostart': autostart}],
arguments=['--ros-args', '--log-level', log_level],
remappings=remappings,
output='screen'),

IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')),
condition=IfCondition(slam),
launch_arguments={'namespace': namespace,
'use_sim_time': use_sim_time,
'autostart': autostart,
'use_respawn': use_respawn,
'params_file': params_file}.items()),

IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir,
'localization_launch.py')),
condition=IfCondition(PythonExpression(['not ', slam])),
launch_arguments={'namespace': namespace,
'map': map_yaml_file,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
'use_composition': use_composition,
'use_respawn': use_respawn,
'container_name': 'nav2_container'}.items()),

IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')),
launch_arguments={'namespace': namespace,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
'use_composition': use_composition,
'use_respawn': use_respawn,
'container_name': 'nav2_container'}.items()),
])

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