From 391793893f3e6a194b412d57b6cd17a442300029 Mon Sep 17 00:00:00 2001 From: Zheng Wang Date: Thu, 24 Oct 2024 16:17:52 -0700 Subject: [PATCH 1/2] add new example --- .../include/nav_two_poses_include.launch.py | 242 ++++++++ .../launch/nav_two_poses.launch.py | 83 +++ .../launch/map_server.launch.py | 47 ++ .../launch/navigation_two_poses.launch.py | 139 +++++ .../maps/idealworks_warehouse_navigation.png | 3 + .../maps/idealworks_warehouse_navigation.yaml | 6 + .../params/nova_carter_two_poses.yaml | 543 ++++++++++++++++++ 7 files changed, 1063 insertions(+) create mode 100644 nova_carter_bringup/launch/include/nav_two_poses_include.launch.py create mode 100644 nova_carter_bringup/launch/nav_two_poses.launch.py create mode 100644 nova_carter_navigation/launch/map_server.launch.py create mode 100644 nova_carter_navigation/launch/navigation_two_poses.launch.py create mode 100644 nova_carter_navigation/maps/idealworks_warehouse_navigation.png create mode 100644 nova_carter_navigation/maps/idealworks_warehouse_navigation.yaml create mode 100644 nova_carter_navigation/params/nova_carter_two_poses.yaml diff --git a/nova_carter_bringup/launch/include/nav_two_poses_include.launch.py b/nova_carter_bringup/launch/include/nav_two_poses_include.launch.py new file mode 100644 index 0000000..0c884f8 --- /dev/null +++ b/nova_carter_bringup/launch/include/nav_two_poses_include.launch.py @@ -0,0 +1,242 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +# flake8: noqa: F403,F405 +import isaac_ros_launch_utils as lu +from isaac_ros_launch_utils.all_types import * +from nav2_common.launch import RewrittenYaml + + +def set_override_parameters(args: lu.ArgumentContainer) -> list[Action]: + local_costmap_plugins = [] + global_costmap_plugins = [] + + # Supported configurations + # - Real Carter + # - A mixture of nvblox, 2D, and 3D LiDAR + # - Isaac Sim Carter + # - nvblox + if args.mode == 'simulation': + # print('In simulation only nvblox costmap supported.') + print('Enabling nvblox local costmap.') + local_costmap_plugins.append('nvblox_layer') + print('Enabling map global costmap.') + global_costmap_plugins.append('static_map_layer') + # global_costmap_plugins.append('nvblox_layer') + odometry_topic = '/visual_slam/tracking/odometry' + else: + enable_global_navigation = lu.is_valid(args.map_yaml_path) + if enable_global_navigation: + print('Enabling global navigation.') + global_costmap_plugins.append('static_map_layer') + + if args.enable_2d_lidar_costmap: + for name in ['front_2d_lidar', 'back_2d_lidar']: + print(f'Enabling 2d lidar costmap for {name}.') + local_costmap_plugins.append(f'{name}_layer') + + if args.enable_3d_lidar_costmap: + print('Enabling 3d lidar costmap.') + local_costmap_plugins.append('3d_lidar_layer') + + if args.enable_nvblox_costmap: + print('Enabling nvblox costmap.') + local_costmap_plugins.append('nvblox_layer') + + if args.enable_wheel_odometry: + odometry_topic = '/chassis/odom' + else: + odometry_topic = '/visual_slam/tracking/odometry' + + local_costmap_plugins += ['inflation_layer'] + global_costmap_plugins += ['inflation_layer'] + + print(f'Using local costmap plugins: {local_costmap_plugins}') + print(f'Using global costmap plugins: {global_costmap_plugins}') + actions: list[Action] = [] + actions.append( + lu.set_parameter( + namespace='/local_costmap/local_costmap', + parameter='plugins', + value=local_costmap_plugins, + )) + actions.append( + lu.set_parameter( + namespace='/global_costmap/global_costmap', + parameter='plugins', + value=global_costmap_plugins, + )) + + actions.append( + lu.set_parameter( + namespace='/bt_navigator', + parameter='odom_topic', + value=odometry_topic, + )) + actions.append( + lu.set_parameter( + namespace='/velocity_smoother', + parameter='odom_topic', + value=odometry_topic, + )) + actions.append( + lu.set_parameter( + namespace='/controller_server', + parameter='odom_topic', + value=odometry_topic, + )) + + if args.enable_3d_lidar_localization: + scan_topic = '/front_3d_lidar/scan' + else: + scan_topic = '/front_2d_lidar/scan' + + actions.append(lu.set_parameter( + namespace='/amcl', + parameter='scan_topic', + value=scan_topic, + )) + + return actions + + +def rewrite_navigation_parameters_for_sim(navigation_parameters_path: LaunchConfiguration, + global_frame: LaunchConfiguration) -> RewrittenYaml: + # For simulation we configure navigation to occur in the VSLAM frame to not clash + # with the Isaac Sim published 'odom' frame. + param_substitutions = {'global_frame': global_frame, 'nav2_costmap_global_frame': global_frame} + return RewrittenYaml(source_file=navigation_parameters_path, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + + +def check_args(args: lu.ArgumentContainer): + # Some of are arguments are not supported when running in Isaac Sim. + # This function checks the passed arguments. + if args.mode == 'simulation': + # if args.map_yaml_path != None: + # print( + # f"Map passed to navigation in Isaac Sim: {args.map_yaml_path}. This feature is not " + # "supported yet. Map ignored, performing local navigation.") + if (args.enable_2d_lidar_costmap == True): + print("Navigation with 2D LiDAR not supported yet in sim. 2D LiDARs will not be used.") + if (args.enable_3d_lidar_costmap == True): + print("Navigation with 3D LiDAR not supported yet in sim. 3D LiDARs will not be used.") + # assert ( + # not args.global_frame == 'odom' + # ), "Tried to use odom as the global frame. This will cause collisions between the Isaac Sim " + # "supplied GT pose and cuVSLAM. Please use another global frame name." + assert (args.stereo_camera_configuration == None + or args.stereo_camera_configuration == 'front_configuration' + ), "Only stereo_camera_configuration:=front_configuration supported in Isaac Sim." + + # Wheel odometry or stereo cameras have to be enabled. + # Otherwise the robot will not have any odometry source + assert (args.enable_wheel_odometry == True or args.stereo_camera_configuration + != None), "Stereo camera(s) or wheel odometry has to be enabled." + + +def generate_launch_description() -> LaunchDescription: + args = lu.ArgumentContainer() + args.add_arg('navigation_parameters_path') + args.add_arg('stereo_camera_configuration', None) + args.add_arg('mode') + args.add_arg('global_frame', 'odom') + args.add_arg('vslam_image_qos', 'DEFAULT') + args.add_arg('map_yaml_path') + args.add_arg('enable_navigation') + args.add_arg('enable_mission_client') + args.add_arg('enable_3d_lidar_costmap') + args.add_arg('enable_2d_lidar_costmap') + args.add_arg('enable_nvblox_costmap') + args.add_arg('enable_wheel_odometry') + args.add_arg('enable_3d_lidar_localization') + + args.add_opaque_function(check_args) + + is_sim = lu.is_equal(args.mode, 'simulation') + + enable_global_navigation = lu.is_valid(args.map_yaml_path) + enable_3d_lidar = OrSubstitution(args.enable_3d_lidar_costmap, + args.enable_3d_lidar_localization) + enable_2d_lidars = OrSubstitution(args.enable_2d_lidar_costmap, + NotSubstitution(args.enable_3d_lidar_localization)) + enabled_2d_lidars = lu.if_else_substitution(enable_2d_lidars, 'front_2d_lidar,back_2d_lidar', + '') + disable_cuvslam = AndSubstitution(args.enable_wheel_odometry, lu.is_not(is_sim)) + + actions = args.get_launch_actions() + + # When running in Isaac Sim we rewrite the parameter file to adjust the frame names. + actions.append(lu.assert_path_exists(args.navigation_parameters_path)) + navigation_parameters_path = args.navigation_parameters_path + + actions.append(SetParametersFromFile(navigation_parameters_path)) + # We have to add the opaque function after having set the default + # parameters. This will also add the opaque function to the actions in + # 'args.get_launch_actions' which we already added above, but we explicitly + # want to only add it now. + actions.append(args.add_opaque_function(set_override_parameters)) + + # Add perceptor. + actions.append( + lu.include( + 'nova_carter_bringup', + 'launch/include/perceptor_include.launch.py', + launch_arguments={ + 'enable_3d_lidar': enable_3d_lidar, + 'enabled_2d_lidars': enabled_2d_lidars, + 'global_frame': args.global_frame, + 'vslam_image_qos': args.vslam_image_qos, + 'invert_odom_to_base_tf': is_sim, + 'disable_cuvslam': disable_cuvslam, + }, + )) + + # Add map server. + actions.append( + lu.include( + 'nova_carter_navigation', + 'launch/map_server.launch.py', + launch_arguments={ + 'map_yaml_path': args.map_yaml_path, + 'navigation_parameters_path': navigation_parameters_path, + # 'enable_3d_lidar_localization': args.enable_3d_lidar_localization, + }, + condition=IfCondition(enable_global_navigation), + )) + actions.append( + lu.static_transform( + 'map', + args.global_frame, + condition=UnlessCondition(enable_global_navigation), + )) + + # Add navigation. + actions.append( + lu.include( + 'nova_carter_navigation', + 'launch/navigation.launch.py', + launch_arguments={ + 'navigation_parameters_path': navigation_parameters_path, + 'enable_mission_client': args.enable_mission_client, + }, + condition=IfCondition(args.enable_navigation), + )) + + return LaunchDescription(actions) diff --git a/nova_carter_bringup/launch/nav_two_poses.launch.py b/nova_carter_bringup/launch/nav_two_poses.launch.py new file mode 100644 index 0000000..76790ef --- /dev/null +++ b/nova_carter_bringup/launch/nav_two_poses.launch.py @@ -0,0 +1,83 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +# flake8: noqa: F403,F405 +import isaac_ros_launch_utils as lu +from isaac_ros_launch_utils.all_types import * + + +def generate_launch_description() -> LaunchDescription: + args = lu.ArgumentContainer() + args.add_arg('mode', 'simulation', cli=True, choices=['real_world', 'simulation', 'rosbag']) + args.add_arg('rosbag', 'None', cli=True) + args.add_arg('navigation_parameters_path', + lu.get_path('nova_carter_navigation', 'params/nova_carter_two_poses.yaml'), + cli=True) + args.add_arg('map_yaml_path', + lu.get_path('nova_carter_navigation', 'maps/idealworks_warehouse_navigation.yaml'), + cli=True) + args.add_arg('enable_navigation', True, cli=True) + args.add_arg('enable_mission_client', False, cli=True) + args.add_arg('enable_3d_lidar_costmap', False, cli=True) + args.add_arg('enable_2d_lidar_costmap', False, cli=True) + args.add_arg('enable_nvblox_costmap', True, cli=True) + args.add_arg('enable_wheel_odometry', True, cli=True) + args.add_arg('enable_3d_lidar_localization', False, cli=True) + args.add_arg('stereo_camera_configuration', + default='front_configuration', + choices=['no_cameras', 'front_configuration', 'front_driver_rectify'], + cli=True) + args.add_arg('enabled_fisheye_cameras', '', cli=True) + args.add_arg('use_foxglove_whitelist', True, cli=True) + args.add_arg('type_negotiation_duration_s', lu.get_default_negotiation_time(), cli=True) + + is_sim = lu.is_equal(args.mode, 'simulation') + + actions = args.get_launch_actions() + actions.append(SetParameter('type_negotiation_duration_s', args.type_negotiation_duration_s)) + actions.append( + lu.log_info(['Using type negotiation duration: ', args.type_negotiation_duration_s])) + actions.append(SetParameter('use_sim_time', True, condition=IfCondition(is_sim))) + + vslam_image_qos = lu.if_else_substitution(is_sim, 'DEFAULT', 'SENSOR_DATA') + + # Add the navigation. + actions.append( + lu.include( + 'nova_carter_bringup', + 'launch/include/nav_two_poses_include.launch.py', + launch_arguments={ + 'global_frame': 'odom_vslam', + 'vslam_image_qos': vslam_image_qos, + }, + )) + + actions.append( + lu.include( + 'isaac_ros_perceptor_bringup', + 'launch/tools/visualization.launch.py', + launch_arguments={'use_foxglove_whitelist': args.use_foxglove_whitelist}, + )) + + # We use a negative niceness to increase the priority. + actions.append(lu.component_container('nova_container', prefix='nice -n -10')) + # A separate container is needed for Nav2 because ROS2 has a race condition for actions when + # using non-isolated containers. + # Nav container has to be added last, else Nav2 parameters are not picked up properly. + actions.append(lu.component_container('navigation_container', container_type='isolated')) + + return LaunchDescription(actions) diff --git a/nova_carter_navigation/launch/map_server.launch.py b/nova_carter_navigation/launch/map_server.launch.py new file mode 100644 index 0000000..05473dd --- /dev/null +++ b/nova_carter_navigation/launch/map_server.launch.py @@ -0,0 +1,47 @@ +import os +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess +from launch.substitutions import LaunchConfiguration +import launch_ros.actions + + +def generate_launch_description(): + ld = LaunchDescription() + + # Map server + map_server_config_path = os.path.join( + get_package_share_directory('nova_carter_navigation'), + 'maps', + 'idealworks_warehouse_navigation.yaml' + ) + + map_server_cmd = Node( + package='nav2_map_server', + executable='map_server', + output='screen', + parameters=[{'yaml_filename': map_server_config_path}], + arguments=['--ros-args', '--log-level', 'info'], + ) + + + lifecycle_nodes = ['map_server'] + use_sim_time = False + autostart = True + + start_lifecycle_manager_cmd = launch_ros.actions.Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + + ld.add_action(map_server_cmd) + ld.add_action(start_lifecycle_manager_cmd) + + return ld \ No newline at end of file diff --git a/nova_carter_navigation/launch/navigation_two_poses.launch.py b/nova_carter_navigation/launch/navigation_two_poses.launch.py new file mode 100644 index 0000000..a4b3363 --- /dev/null +++ b/nova_carter_navigation/launch/navigation_two_poses.launch.py @@ -0,0 +1,139 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +# flake8: noqa: F403,F405 +import isaac_ros_launch_utils as lu +from isaac_ros_launch_utils.all_types import * + + +def generate_launch_description() -> LaunchDescription: + args = lu.ArgumentContainer() + args.add_arg('enable_mission_client') + args.add_arg('navigation_parameters_path') + args.add_arg('navigation_container_name', 'navigation_container') + + actions = args.get_launch_actions() + actions.append( + lu.include( + 'nav2_bringup', + 'launch/navigation_launch.py', + launch_arguments={ + # The parameters file here does not work correctly when using + # composition. In addition to here it also has to be loaded with + # `SetParameterFromFile` before the corresponding + # component_container is added to the launch description. + 'params_file': args.navigation_parameters_path, + 'container_name': args.navigation_container_name, + # NOTE: nav2 wants this parameter as a capitalized string. + 'use_composition': 'True', + }, + )) + + actions.append( + lu.include( + 'isaac_ros_vda5050_nav2_client_bringup', + 'launch/isaac_ros_vda5050_client.launch.py', + launch_arguments={ + 'docking_server_enabled': 'True', + }, + condition=IfCondition(args.enable_mission_client), + )) + + switch_node = ComposableNode( + package='custom_nitros_image', + plugin='nvidia::isaac_ros::custom_nitros_image::NitrosImageSwitchNode', + name='switch_node', + remappings=[ + ('image', '/front_stereo_camera/left/image_rect'), + ('camera_info', '/front_stereo_camera/left/camera_info_rect'), + ('switched_image', '/front_stereo_camera/left/switched_image_rect'), + ('switched_camera_info', '/front_stereo_camera/left/switched_camera_info_rect'), + ], + parameters=[{ + 'initial_switch_state': False + }], + condition=IfCondition(args.enable_mission_client), + ) + apriltag_node = ComposableNode( + package='isaac_ros_apriltag', + plugin='nvidia::isaac_ros::apriltag::AprilTagNode', + name='apriltag', + remappings=[('image', '/front_stereo_camera/left/switched_image_rect'), + ('camera_info', '/front_stereo_camera/left/switched_camera_info_rect')], + condition=IfCondition(args.enable_mission_client), + parameters=[{ + 'size': 0.1524, # 6 inches + 'max_tags': 4, + 'tile_size': 4, + }], + ) + load_nodes = lu.load_composable_nodes( + args.navigation_container_name, + [switch_node, apriltag_node], + condition=IfCondition(args.enable_mission_client), + ) + actions.append(load_nodes) + + # docking_server = Node( + # package='opennav_docking', + # executable='opennav_docking', + # name='docking_server', + # output='screen', + # parameters=[str(lu.get_path('nova_carter_docking', 'params/nova_carter_docking.yaml'))], + # condition=IfCondition(args.enable_mission_client), + # ) + + map_server_node = Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=False, + respawn_delay=2.0, + parameters=[ + str(lu.get_path('nova_carter_navigation', 'params/nova_carter_two_poses.yaml')) + ], + arguments=['--ros-args', '--log-level', 'info'], + remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], + ) + + lifecycle_manager = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_docking', + output='screen', + parameters=[{ + 'autostart': True + }, { + 'node_names': ['map_server_node'] + }], + # condition=IfCondition(args.enable_mission_client), + ) + + dock_pose_publisher = Node( + package='nova_carter_docking', + executable='dock_pose_publisher', + name='dock_pose_publisher', + parameters=[{ + 'use_first_detection': True, + 'dock_tag_id': 586, + }], + condition=IfCondition(args.enable_mission_client), + ) + actions.extend([map_server_node, lifecycle_manager, dock_pose_publisher]) + + return LaunchDescription(actions) diff --git a/nova_carter_navigation/maps/idealworks_warehouse_navigation.png b/nova_carter_navigation/maps/idealworks_warehouse_navigation.png new file mode 100644 index 0000000..ec0b144 --- /dev/null +++ b/nova_carter_navigation/maps/idealworks_warehouse_navigation.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5e93a7d9ef72c40d1af988cdc87473577225db8d6daf5b74ef2a1a00dd6ce80d +size 11112 diff --git a/nova_carter_navigation/maps/idealworks_warehouse_navigation.yaml b/nova_carter_navigation/maps/idealworks_warehouse_navigation.yaml new file mode 100644 index 0000000..48757b5 --- /dev/null +++ b/nova_carter_navigation/maps/idealworks_warehouse_navigation.yaml @@ -0,0 +1,6 @@ +image: idealworks_warehouse_navigation.png +resolution: 0.05 +origin: [-27.975, -41.37500152587891, 0.0000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/nova_carter_navigation/params/nova_carter_two_poses.yaml b/nova_carter_navigation/params/nova_carter_two_poses.yaml new file mode 100644 index 0000000..945898d --- /dev/null +++ b/nova_carter_navigation/params/nova_carter_two_poses.yaml @@ -0,0 +1,543 @@ +slam_toolbox: + ros__parameters: + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /front_3d_lidar/scan + mode: mapping + # lifelong params + lifelong_search_use_tree: false + lifelong_minimum_score: 0.1 + lifelong_iou_match: 0.85 + lifelong_node_removal_score: 0.04 + lifelong_overlap_score_scale: 0.06 + lifelong_constraint_multiplier: 0.08 + lifelong_nearby_penalty: 0.001 + lifelong_candidates_scale: 0.03 + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 10. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.1 + alpha2: 0.1 + alpha3: 0.1 + alpha4: 0.1 + alpha5: 0.1 + base_frame_id: "base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 4.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "beam" + max_beams: 360 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.1 + tf_broadcast: true + transform_tolerance: 3.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.4 + z_short: 0.05 +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + bt_loop_duration: 10 + default_server_timeout: 20 + navigators: ['navigate_to_pose', 'navigate_through_poses'] + default_nav_to_pose_bt_xml: $(find-pkg-share nova_carter_navigation)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml + default_nav_through_poses_bt_xml: $(find-pkg-share nova_carter_navigation)/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml + navigate_to_pose: + plugin: "nav2_bt_navigator/NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["stopped_goal_checker"] + controller_plugins: ["FollowPath"] + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + stopped_goal_checker: + plugin: "nav2_controller::StoppedGoalChecker" + trans_stopped_velocity: 0.1 + rot_stopped_velocity: 0.1 + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + vx_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: false + reset_period: 1.0 # (only in Humble) + regenerate_noises: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + ObstaclesCritic: + enabled: true + cost_power: 1 + repulsion_weight: 4.0 + critical_weight: 20.0 + consider_footprint: True + collision_cost: 10000.0 + collision_margin_distance: 0.1 + near_goal_distance: 0.5 + inflation_radius: 0.8 # (only in Humble) + cost_scaling_factor: 10.0 # (only in Humble) + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 3 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 +local_costmap: + local_costmap: + ros__parameters: + footprint_padding: 0.03 + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom_vslam + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + transform_tolerance: 0.3 + footprint: "[ [0.14, 0.25], [0.14, -0.25], [-0.607, -0.25], [-0.607, 0.25] ]" + mark_threshold: 2 + always_send_full_costmap: True + plugins: [TO_BE_OVERRIDDEN] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + enabled: True + cost_scaling_factor: 10.0 + inflation_radius: 0.8 + front_2d_lidar_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /front_2d_lidar/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + back_2d_lidar_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /back_2d_lidar/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + nvblox_layer: + plugin: "nvblox::nav2::NvbloxCostmapLayer" + enabled: True + nav2_costmap_global_frame: odom_vslam # must match with global_frame of local_costmap + nvblox_map_slice_topic: "/nvblox_node/static_map_slice" + convert_to_binary_costmap: True + 3d_lidar_layer: + plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" + enabled: true + voxel_decay: 15.0 # seconds if linear, e^n if exponential + decay_model: 0 # 0=linear, 1=exponential, -1=persistent + voxel_size: 0.05 # meters + track_unknown_space: False # default space is known + mark_threshold: 2 # voxel height + update_footprint_enabled: true + combination_method: 1 # 1=max, 0=override + origin_z: 0.0 # meters + publish_voxel_map: true # default off + transform_tolerance: 0.2 # seconds + observation_sources: hesai_mark hesai_clear + hesai_mark: + data_type: PointCloud2 + topic: /front_3d_lidar/pointcloud + marking: true + clearing: false + obstacle_range: 3.0 # meters + min_obstacle_height: 0.1 # default 0, meters + max_obstacle_height: 1.5 # default 3, meters + expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer + observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest + inf_is_valid: false # default false, for laser scans + filter: "voxel" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter + clear_after_reading: true # default false, clear the buffer after the layer gets readings from it + hesai_clear: + enabled: true #default true, can be toggled on/off with associated service call + data_type: PointCloud2 + topic: /front_3d_lidar/pointcloud + marking: false + clearing: true + max_z: 20.0 # default 0, meters + min_z: 0.05 # default 10, meters + vertical_fov_angle: 1.15 # Note: This the full fov. + vertical_fov_padding: 0.05 # 3D Lidar only. Default 0, in meters + horizontal_fov_angle: 6.29 # 3D lidar scanners like the VLP16 have 360 deg horizontal FOV. + decay_acceleration: 5.0 # default 0, 1/s^2. + model_type: 1 # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar +global_costmap: + global_costmap: + ros__parameters: + footprint_padding: 0.25 + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + footprint: "[ [0.14, 0.25], [0.14, -0.25], [-0.607, -0.25], [-0.607, 0.25] ]" + resolution: 0.05 + # The following is only used as a default when no map is specified. + # width: 50 + # height: 50 + # origin_x: -25.0 + # origin_y: -25.0 + # track_unknown_space: False + # mark_threshold: 2 + always_send_full_costmap: True + plugins: [TO_BE_OVERRIDDEN] + static_map_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + enabled: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + enabled: True + cost_scaling_factor: 10.0 + inflation_radius: 0.8 + front_2d_lidar_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /front_2d_lidar/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + back_2d_lidar_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /back_2d_lidar/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + nvblox_layer: + plugin: "nvblox::nav2::NvbloxCostmapLayer" + enabled: True + nav2_costmap_global_frame: map # must match with global_frame of global_costmap + nvblox_map_slice_topic: "/nvblox_node/static_map_slice" + convert_to_binary_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "/workspaces/isaac_ros-dev/install/nova_carter_navigation/share/nova_carter_navigation/maps/idealworks_warehouse_navigation.yaml" +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True +planner_server: + ros__parameters: + expected_planner_frequency: 1.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # max time in s for planner to plan, smooth + motion_model_for_search: "REEDS_SHEPP" # Hybrid-A* Dubin, Redds-Shepp + angle_quantization_bins: 72 # Number of angle bins for search + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 1.5 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + minimum_turning_radius: 0.20 # minimum turning radius in m of path / vehicle + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 3.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + use_quadratic_cost_penalty: False + downsample_obstacle_heuristic: True + allow_primitive_interpolation: False + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True +behavior_server: + ros__parameters: + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + local_frame: odom_vslam + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: True + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 +robot_state_publisher: + ros__parameters: + use_sim_time: True +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.7, 0.0, 1.0] + min_velocity: [-0.7, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 From 2781bd7de4bd381bba1d0be937721466faccd3c7 Mon Sep 17 00:00:00 2001 From: Zheng Wang Date: Thu, 24 Oct 2024 18:06:19 -0700 Subject: [PATCH 2/2] add idealworks_bringup package --- idealworks_bringup/CMakeLists.txt | 46 ++++++ idealworks_bringup/package.xml | 26 ++++ idealworks_bringup/src/iw_navigation.cpp | 184 +++++++++++++++++++++++ 3 files changed, 256 insertions(+) create mode 100644 idealworks_bringup/CMakeLists.txt create mode 100644 idealworks_bringup/package.xml create mode 100644 idealworks_bringup/src/iw_navigation.cpp diff --git a/idealworks_bringup/CMakeLists.txt b/idealworks_bringup/CMakeLists.txt new file mode 100644 index 0000000..0aa0497 --- /dev/null +++ b/idealworks_bringup/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.8) +project(idealworks_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(rosgraph_msgs REQUIRED) + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(iw_navigation src/iw_navigation.cpp) +ament_target_dependencies(iw_navigation + rclcpp std_msgs nav_msgs std_srvs geometry_msgs rosgraph_msgs tf2 tf2_ros) +target_include_directories(iw_navigation PUBLIC + $ + $) +target_compile_features(iw_navigation PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS iw_navigation + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/idealworks_bringup/package.xml b/idealworks_bringup/package.xml new file mode 100644 index 0000000..842aba3 --- /dev/null +++ b/idealworks_bringup/package.xml @@ -0,0 +1,26 @@ + + + + idealworks_bringup + 0.0.0 + TODO: Package description + zhengwang + TODO: License declaration + + ament_cmake + rclcpp + std_msgs + std_srvs + nav_msgs + geometry_msgs + tf2 + tf2_ros + rosgraph_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/idealworks_bringup/src/iw_navigation.cpp b/idealworks_bringup/src/iw_navigation.cpp new file mode 100644 index 0000000..b96a862 --- /dev/null +++ b/idealworks_bringup/src/iw_navigation.cpp @@ -0,0 +1,184 @@ +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "rosgraph_msgs/msg/clock.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/convert.h" +#include "tf2_ros/buffer.h" + +using std::placeholders::_1; + +using namespace std::chrono_literals; + +class TestNode : public rclcpp::Node +{ + public: + TestNode() + : Node("iw_navigation") + { + goal1.header.frame_id = "odom_vslam"; + goal1.pose.position.x = 3; + goal1.pose.position.y = 3; + goal1.pose.position.z = 0; + goal1.pose.orientation.x = 0; + goal1.pose.orientation.y = 0; + goal1.pose.orientation.z = 0; + goal1.pose.orientation.w = 1; + + goal2.header.frame_id = "odom_vslam"; + goal2.pose.position.x = -3; + goal2.pose.position.y = -3; + goal2.pose.position.z = 0; + goal2.pose.orientation.x = 0; + goal2.pose.orientation.y = 0; + goal2.pose.orientation.z = 0; + goal2.pose.orientation.w = 1; + + index = 0; + reachedGoal = true; + threshold = 0.5; + navRequest.store(false); + + subscription_ = this->create_subscription( + "/visual_slam/vis/slam_odometry", 10, + std::bind(&TestNode::PoseCallback, this, _1)); + subscription_clock_ = this->create_subscription( + "/clock", 10, + std::bind(&TestNode::publishMap2Odom, this, _1)); + publisher_ = this->create_publisher( + "/goal_pose", 10); + service_ = this->create_service( + "set_navigation", + std::bind(&TestNode::NavRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + tf_broadcaster_ = std::make_shared(this); + + goal_pose_timer_ = this->create_wall_timer( + 3s, std::bind(&TestNode::GoalPoseCallback, this)); + } + + private: + void publishMap2Odom(const rosgraph_msgs::msg::Clock& curTime) + { + // Create a TransformStamped message + geometry_msgs::msg::TransformStamped transform; + tf2::TimePoint transform_expiration = tf2_ros::fromMsg(curTime.clock) + tf2::durationFromSec(0.0); + transform.header.stamp = tf2_ros::toMsg(transform_expiration); + RCLCPP_INFO(this->get_logger(), "Published transform for time stamp %d" , transform.header.stamp.sec); + transform.header.frame_id = "map"; + transform.child_frame_id = "odom"; + + // Set translation (x, y, z) + transform.transform.translation.x = -6.36564; // Example translation + transform.transform.translation.y = 1.87924; // Example translation + transform.transform.translation.z = 0.0; + + // Set rotation (x, y, z, w) + tf2::Quaternion q; + q.setRPY(0.0, 0.0, 0.0); // Roll, Pitch, Yaw + transform.transform.rotation.x = q.x(); + transform.transform.rotation.y = q.y(); + transform.transform.rotation.z = q.z(); + transform.transform.rotation.w = q.w(); + + // Send the transform + tf_broadcaster_->sendTransform(transform); + // RCLCPP_INFO(this->get_logger(), "Published transform from map to odom"); + } + void NavRequestCallback(const std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_INFO(this->get_logger(), "Request received: %s", request->data ? "true" : "false"); + + // Process the request (e.g., just echoing the value back) + navRequest.store(request->data); + response->success = true; // You can set this based on your logic + response->message = request->data ? "Start navigation" : "Stop navigation"; + + RCLCPP_INFO(this->get_logger(), "Response sent: %s", response->message.c_str()); + } + + void PoseCallback(const nav_msgs::msg::Odometry::SharedPtr msg) + { + if (!navRequest.load()) return; + std::lock_guard lock(poseMutex); + // RCLCPP_INFO(this->get_logger(), + // "received pose: %f, %f, %f", + // msg->pose.pose.position.x, + // msg->pose.pose.position.y, + // msg->pose.pose.position.z); + if (reachedGoal) + return; + + if (index == 0 && std::fabs(msg->pose.pose.position.x - goal1.pose.position.x) < threshold && + std::fabs(msg->pose.pose.position.y - goal1.pose.position.y) < threshold && + std::fabs(msg->pose.pose.position.z - goal1.pose.position.z) < threshold) + { + RCLCPP_INFO(this->get_logger(), "Reached goal pose1"); + reachedGoal = true; + index = 1; + } + else if (index == 1 && std::fabs(msg->pose.pose.position.x - goal2.pose.position.x) < threshold && + std::fabs(msg->pose.pose.position.y - goal2.pose.position.y) < threshold && + std::fabs(msg->pose.pose.position.z - goal2.pose.position.z) < threshold) + { + RCLCPP_INFO(this->get_logger(), "Reached goal pose2"); + reachedGoal = true; + index = 0; + } + } + void GoalPoseCallback() + { + if (!navRequest.load()) return; + std::lock_guard lock(poseMutex); + if (!reachedGoal) + { + return; + } + + if (index == 0) + { + RCLCPP_INFO(this->get_logger(), "Publishing: goal pose1"); + publisher_->publish(goal1); + reachedGoal = false; + } + else if (index == 1) + { + RCLCPP_INFO(this->get_logger(), "Publishing: goal pose2"); + publisher_->publish(goal2); + reachedGoal = false; + } + } + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr subscription_clock_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Service::SharedPtr service_; + std::shared_ptr tf_broadcaster_; + rclcpp::TimerBase::SharedPtr goal_pose_timer_; + geometry_msgs::msg::PoseStamped goal1, goal2; + int index; + bool reachedGoal; + float threshold; + std::mutex poseMutex; + std::atomic navRequest; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file