diff --git a/nav2_gps_waypoint_follower_demo/config/dual_ekf_navsat_params.yaml b/nav2_gps_waypoint_follower_demo/config/dual_ekf_navsat_params.yaml index 5358f15..8f6d9a0 100644 --- a/nav2_gps_waypoint_follower_demo/config/dual_ekf_navsat_params.yaml +++ b/nav2_gps_waypoint_follower_demo/config/dual_ekf_navsat_params.yaml @@ -10,10 +10,10 @@ ekf_filter_node_odom: map_frame: map odom_frame: odom - base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin + base_link_frame: base_link # the frame id used by the turtlebot's diff drive plugin world_frame: odom - odom0: odom + odom0: /demo/odom odom0_config: [false, false, false, false, false, false, true, true, true, @@ -23,7 +23,7 @@ ekf_filter_node_odom: odom0_differential: false odom0_relative: false - imu0: imu + imu0: demo/imu imu0_config: [false, false, false, false, false, true, false, false, false, @@ -62,10 +62,10 @@ ekf_filter_node_map: map_frame: map odom_frame: odom - base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin + base_link_frame: base_link # the frame id used by the turtlebot's diff drive plugin world_frame: map - odom0: odom + odom0: /demo/odom odom0_config: [false, false, false, false, false, false, true, true, true, @@ -85,7 +85,7 @@ ekf_filter_node_map: odom1_differential: false odom1_relative: false - imu0: imu + imu0: /demo/imu imu0_config: [false, false, false, false, false, true, false, false, false, diff --git a/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml b/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml index 31fad8f..18b12a0 100644 --- a/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml +++ b/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml @@ -10,70 +10,70 @@ bt_navigator: ros__parameters: global_frame: map robot_base_frame: base_link - odom_topic: /odom + odom_topic: /odometry/global bt_loop_duration: 10 default_server_timeout: 20 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: - plugin: "nav2_bt_navigator/NavigateToPoseNavigator" + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" navigate_through_poses: - plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" + 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_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_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 + # 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_are_error_codes_active_condition_bt_node + # - nav2_would_a_controller_recovery_help_condition_bt_node + # - nav2_would_a_planner_recovery_help_condition_bt_node + # - nav2_would_a_smoother_recovery_help_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 error_code_names: - compute_path_error_code - follow_path_error_code @@ -249,7 +249,7 @@ planner_server: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" + plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true @@ -272,15 +272,15 @@ behavior_server: cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_behaviors/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_behaviors/BackUp" + plugin: "nav2_behaviors::BackUp" drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" + plugin: "nav2_behaviors::DriveOnHeading" wait: - plugin: "nav2_behaviors/Wait" + plugin: "nav2_behaviors::Wait" assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" + plugin: "nav2_behaviors::AssistedTeleop" local_frame: odom global_frame: map robot_base_frame: base_link @@ -309,7 +309,96 @@ velocity_smoother: min_velocity: [-0.26, 0.0, -1.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" + odom_topic: "odometry/global" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + + + +docking_server: + ros__parameters: + controller_frequency: 10.0 + initial_perception_timeout: 20.0 # Default 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.05 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['rosmaster_x3_dock'] + rosmaster_x3_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.02 + staging_x_offset: 0.75 + staging_yaw_offset: 3.14 + use_external_detection_pose: true + use_battery_status: false + use_stall_detection: false + stall_velocity_threshold: 1.0 + stall_effort_threshold: 1.0 + charging_threshold: 0.5 + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: 1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # dock_database: $(find-pkg-share yahboom_rosmaster_docking)/config/dock_database.yaml + + controller: + k_phi: 3.0 + k_delta: 2.0 + beta: 0.4 + lambda: 2.0 + v_linear_min: 0.1 + v_linear_max: 0.15 + v_angular_max: 0.75 + slowdown_radius: 0.25 + use_collision_detection: true + costmap_topic: "/local_costmap/costmap_raw" + footprint_topic: "/local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 \ No newline at end of file diff --git a/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py b/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py index 4bf4449..6f94d21 100644 --- a/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py +++ b/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py @@ -1,82 +1,83 @@ -# Copyright (c) 2018 Intel Corporation -# -# 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. - import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ExecuteProcess, SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration from launch_ros.actions import Node +from ros_gz_bridge.actions import RosGzBridge +from ros_gz_sim.actions import GzServer def generate_launch_description(): - # Get the launch directory - gps_wpf_dir = get_package_share_directory( - "nav2_gps_waypoint_follower_demo") - launch_dir = os.path.join(gps_wpf_dir, 'launch') - world = os.path.join(gps_wpf_dir, "worlds", "sonoma_raceway.world") - - urdf = os.path.join(gps_wpf_dir, 'urdf', 'turtlebot3_waffle_gps.urdf') - with open(urdf, 'r') as infp: - robot_description = infp.read() - - models_dir = os.path.join(gps_wpf_dir, "models") - models_dir += os.pathsep + \ - f"/opt/ros/{os.getenv('ROS_DISTRO')}/share/turtlebot3_gazebo/models" - set_gazebo_model_path_cmd = None - - if 'GAZEBO_MODEL_PATH' in os.environ: - gazebo_model_path = os.environ['GAZEBO_MODEL_PATH'] + \ - os.pathsep + models_dir - set_gazebo_model_path_cmd = SetEnvironmentVariable( - "GAZEBO_MODEL_PATH", gazebo_model_path) - else: - set_gazebo_model_path_cmd = SetEnvironmentVariable( - "GAZEBO_MODEL_PATH", models_dir) - - set_tb3_model_cmd = SetEnvironmentVariable("TURTLEBOT3_MODEL", "waffle") - - # Specify the actions - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '-s', 'libgazebo_ros_factory.so', world], - cwd=[launch_dir], output='both') - - start_gazebo_client_cmd = ExecuteProcess( - cmd=['gzclient'], - cwd=[launch_dir], output='both') - - start_robot_state_publisher_cmd = Node( + world_pkg_share = get_package_share_directory('nav2_gps_waypoint_follower_demo') + robot_pkg_share = get_package_share_directory('sam_bot_description') + ros_gz_sim_share = get_package_share_directory('ros_gz_sim') + gz_spawn_model_launch_source = os.path.join(ros_gz_sim_share, "launch", "gz_spawn_model.launch.py") + default_model_path = os.path.join(robot_pkg_share, 'src', 'description', 'sam_bot_description.sdf') + world_path = os.path.join(world_pkg_share, 'worlds', 'sonoma_world.sdf') + bridge_config_path = os.path.join(robot_pkg_share, 'config', 'bridge_config.yaml') + + robot_state_publisher_node = Node( package='robot_state_publisher', executable='robot_state_publisher', - name='robot_state_publisher', - output='both', - parameters=[{'robot_description': robot_description}]) - - # Create the launch description and populate - ld = LaunchDescription() - - # Set gazebo up to find models properly - ld.add_action(set_gazebo_model_path_cmd) - ld.add_action(set_tb3_model_cmd) - - # simulator launch - ld.add_action(start_gazebo_server_cmd) - ld.add_action(start_gazebo_client_cmd) - - # robot state publisher launch - ld.add_action(start_robot_state_publisher_cmd) - - return ld + parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}, {'use_sim_time': LaunchConfiguration('use_sim_time')}] + ) + + gz_server = GzServer( + world_sdf_file=world_path, + container_name='ros_gz_container', + create_own_container='True', + use_composition='True', + ) + ros_gz_bridge = RosGzBridge( + bridge_name='ros_gz_bridge', + config_file=bridge_config_path, + container_name='ros_gz_container', + + create_own_container='False', + use_composition='True', + ) + camera_bridge_image = Node( + package='ros_gz_image', + executable='image_bridge', + name='bridge_gz_ros_camera_image', + output='screen', + parameters=[{'use_sim_time': True}], + arguments=['/depth_camera/image'], + ) + camera_bridge_depth = Node( + package='ros_gz_image', + executable='image_bridge', + name='bridge_gz_ros_camera_depth', + output='screen', + parameters=[{'use_sim_time': True}], + arguments=['/depth_camera/depth_image'], + ) + spawn_entity = IncludeLaunchDescription( + PythonLaunchDescriptionSource(gz_spawn_model_launch_source), + launch_arguments={ + 'world': 'my_world', + 'topic': '/robot_description', + 'entity_name': 'sam_bot', + 'x': '2.0', # meters + 'y': '-2.5', + 'z': '0.65', + 'R': '0.0', # roll (radians) + 'P': '0.0', # pitch + 'Y': '0.0', # yaw + }.items(), + ) + + return LaunchDescription([ + DeclareLaunchArgument(name='model', default_value=default_model_path, description='Absolute path to robot model file'), + DeclareLaunchArgument(name='use_sim_time', default_value='True', description='Flag to enable use_sim_time'), + ExecuteProcess(cmd=['gz', 'sim', '-g'], output='screen'), + robot_state_publisher_node, + gz_server, + ros_gz_bridge, + camera_bridge_image, + camera_bridge_depth, + spawn_entity, + ]) diff --git a/nav2_gps_waypoint_follower_demo/launch/gps_waypoint_follower.launch.py b/nav2_gps_waypoint_follower_demo/launch/gps_waypoint_follower.launch.py index 279fafd..36e247e 100644 --- a/nav2_gps_waypoint_follower_demo/launch/gps_waypoint_follower.launch.py +++ b/nav2_gps_waypoint_follower_demo/launch/gps_waypoint_follower.launch.py @@ -41,7 +41,7 @@ def generate_launch_description(): declare_use_rviz_cmd = DeclareLaunchArgument( 'use_rviz', - default_value='False', + default_value='true', description='Whether to start RVIZ') declare_use_mapviz_cmd = DeclareLaunchArgument( diff --git a/nav2_gps_waypoint_follower_demo/worlds/sonoma_world.sdf b/nav2_gps_waypoint_follower_demo/worlds/sonoma_world.sdf new file mode 100644 index 0000000..c6377b8 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/worlds/sonoma_world.sdf @@ -0,0 +1,88 @@ + + + + 0.001 + 1.0 + + + + + + + + + + + ogre2 + + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + + EARTH_WGS84 + 38.161479 + -122.454630 + 488.0 + + + + -287.5 143.5 -7 0 0 0 + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sonoma Raceway + + + + + 3.17226 -5.10401 6.58845 0 0.739643 2.19219 + orbit + perspective + + + + diff --git a/sam_bot_description/config/bridge_config.yaml b/sam_bot_description/config/bridge_config.yaml index d539047..678086a 100644 --- a/sam_bot_description/config/bridge_config.yaml +++ b/sam_bot_description/config/bridge_config.yaml @@ -26,9 +26,9 @@ direction: GZ_TO_ROS # Topic subscribed to by DiffDrive plugin -- ros_topic_name: "/demo/cmd_vel" +- ros_topic_name: "/cmd_vel" gz_topic_name: "/demo/cmd_vel" - ros_type_name: "geometry_msgs/msg/TwistStamped" + ros_type_name: "geometry_msgs/msg/Twist" gz_type_name: "gz.msgs.Twist" direction: ROS_TO_GZ @@ -55,3 +55,13 @@ ros_type_name: "sensor_msgs/msg/PointCloud2" gz_type_name: "gz.msgs.PointCloudPacked" direction: GZ_TO_ROS + +- topic_name: "gps/fix" + ros_type_name: "sensor_msgs/msg/NavSatFix" + gz_type_name: "gz.msgs.NavSat" + direction: GZ_TO_ROS + +# - topic_name: "tf" +# ros_type_name: "tf2_msgs/msg/TFMessage" +# gz_type_name: "gz.msgs.Pose_V" +# direction: GZ_TO_ROS \ No newline at end of file diff --git a/sam_bot_description/src/description/sam_bot_description.sdf b/sam_bot_description/src/description/sam_bot_description.sdf index 68f5597..80aa619 100644 --- a/sam_bot_description/src/description/sam_bot_description.sdf +++ b/sam_bot_description/src/description/sam_bot_description.sdf @@ -188,7 +188,6 @@ imu_link 0.0 0.0 0.01 0 0 0 - @@ -271,7 +270,50 @@ - + + base_link + gps_link + 0.0 0.0 0.3 0 0 0 + + + + + -0.052 0 0.111 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + true + 1 + 0 0 0 0 0 0 + gps/fix + gps_link + + + + + 0.0 + 0.01 + + + + + 0.0 + 0.01 + + + + + + + drivewhl_l_joint @@ -349,11 +391,11 @@ 3.5 0.015000 - + diff --git a/sam_bot_description/world/my_world.sdf b/sam_bot_description/world/my_world.sdf index 66ce5a0..e77f097 100644 --- a/sam_bot_description/world/my_world.sdf +++ b/sam_bot_description/world/my_world.sdf @@ -24,6 +24,10 @@ name="gz::sim::systems::Sensors"> ogre2 + + 1 0 0 10 0 -0 0 @@ -104,11 +108,15 @@ + EARTH_WGS84 - 0 - 0 - 0 - 0 + 38.161479 + -122.454630 + 488.0 + 180 1.51271 -0.181418 0.5 0 -0 0