diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst index a41ee5a7a..52a8dc89d 100644 --- a/webots_ros2/CHANGELOG.rst +++ b/webots_ros2/CHANGELOG.rst @@ -13,6 +13,8 @@ Changelog for package webots_ros2 * Changed undefined Lidar frequency to the default from the .proto file * Added Compass support in webots_ros2_driver * Added startup of the Turlebot navigation and mapping tools from the launch file. +* Fixed the calibration of the e-puck. +* Fixed and improved the navigation of the e-puck example. 2023.0.2 (2023-02-07) ------------------ diff --git a/webots_ros2_epuck/CHANGELOG.rst b/webots_ros2_epuck/CHANGELOG.rst index 668befdd1..4b70a025c 100644 --- a/webots_ros2_epuck/CHANGELOG.rst +++ b/webots_ros2_epuck/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package webots_ros2_epuck ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.0.3 (2023-XX-XX) +------------------ +* Refactored launch files. +* Updated and fixed navigation for Humble compatibility. + 2023.0.2 (2023-02-07) ------------------ * Updated supervisor launch. diff --git a/webots_ros2_epuck/launch/rats_life_launch.py b/webots_ros2_epuck/launch/rats_life_launch.py index 2cb4d5be9..7a6787195 100644 --- a/webots_ros2_epuck/launch/rats_life_launch.py +++ b/webots_ros2_epuck/launch/rats_life_launch.py @@ -23,14 +23,12 @@ from launch import LaunchDescription from ament_index_python.packages import get_package_share_directory, get_packages_with_prefixes from launch_ros.actions import Node -from launch.actions import ExecuteProcess def generate_launch_description(): launch_description_nodes = [] package_dir = get_package_share_directory('webots_ros2_epuck') use_sim_time = LaunchConfiguration('use_sim_time', default=True) - synchronization = LaunchConfiguration('synchronization', default=True) world = LaunchConfiguration('world', default='rats_life_benchmark.wbt') # Webots node @@ -40,7 +38,6 @@ def generate_launch_description(): os.path.join(package_dir, 'launch', 'robot_launch.py') ), launch_arguments={ - 'synchronization': synchronization, 'use_sim_time': 'true', 'world': world }.items() @@ -76,25 +73,6 @@ def generate_launch_description(): ], ) ) - # Set initial position of the robot within the provided map. - # The initial position can be also be set in RViz2 menu `2D Pose Estimate`. - launch_description_nodes.append(ExecuteProcess( - cmd=[ - 'ros2', - 'topic', - 'pub', - '--once', - '/initialpose', - 'geometry_msgs/msg/PoseWithCovarianceStamped', - '{\ - "header": { "frame_id": "map" },\ - "pose": { "pose": {\ - "position": { "x": 0.005, "y": 0.0, "z": 0.0 },\ - "orientation": { "x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0 }}\ - }\ - }' - ] - )) else: launch_description_nodes.append(LogInfo(msg='Navigation2 is not installed, navigation functionality is disabled')) diff --git a/webots_ros2_epuck/launch/rats_life_waypoints_launch.py b/webots_ros2_epuck/launch/rats_life_waypoints_launch.py index b3eaeab48..c1416638b 100644 --- a/webots_ros2_epuck/launch/rats_life_waypoints_launch.py +++ b/webots_ros2_epuck/launch/rats_life_waypoints_launch.py @@ -101,7 +101,6 @@ def get_waypoints(): def generate_launch_description(): package_dir = get_package_share_directory('webots_ros2_epuck') use_sim_time = LaunchConfiguration('use_sim_time', default=True) - synchronization = LaunchConfiguration('synchronization', default=True) world = LaunchConfiguration('world', default='rats_life_benchmark.wbt') # Webots @@ -110,7 +109,6 @@ def generate_launch_description(): os.path.join(package_dir, 'launch', 'robot_launch.py') ), launch_arguments={ - 'synchronization': synchronization, 'use_sim_time': 'true', 'world': world }.items() @@ -153,7 +151,7 @@ def generate_launch_description(): cmd=[ 'sleep 5;' 'while [ -z ' - f'`ros2 action send_goal /FollowWaypoints nav2_msgs/action/FollowWaypoints \'{get_waypoints()}\' | grep accepted`' + f'`ros2 action send_goal /follow_waypoints nav2_msgs/action/FollowWaypoints \'{get_waypoints()}\' | grep accepted`' ']; do' ' sleep 3;' 'done' diff --git a/webots_ros2_epuck/launch/robot_launch.py b/webots_ros2_epuck/launch/robot_launch.py index 69a33e7a9..5718d534a 100644 --- a/webots_ros2_epuck/launch/robot_launch.py +++ b/webots_ros2_epuck/launch/robot_launch.py @@ -20,10 +20,11 @@ import pathlib import launch from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.substitutions.path_join_substitution import PathJoinSubstitution from launch_ros.actions import Node from launch import LaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory from webots_ros2_driver.webots_launcher import WebotsLauncher from webots_ros2_driver.utils import controller_url_prefix @@ -31,6 +32,11 @@ def get_ros2_nodes(*args): package_dir = get_package_share_directory('webots_ros2_epuck') + use_nav = LaunchConfiguration('nav', default=False) + use_rviz = LaunchConfiguration('rviz', default=False) + use_mapper = LaunchConfiguration('mapper', default=False) + fill_map = LaunchConfiguration('fill_map', default=True) + map_filename = LaunchConfiguration('map', default=os.path.join(package_dir, 'resource', 'epuck_world_map.yaml')) robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'epuck_webots.urdf')).read_text() ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml') use_sim_time = LaunchConfiguration('use_sim_time', default=True) @@ -104,6 +110,45 @@ def get_ros2_nodes(*args): arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'], ) + tool_nodes = [] + # Navigation + nav_tools = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(package_dir, 'launch', 'robot_tools_launch.py') + ), + launch_arguments={ + 'nav': use_nav, + 'rviz': use_rviz, + 'use_sim_time': use_sim_time, + 'map': map_filename, + }.items(), + condition=launch.conditions.IfCondition(use_nav) + ) + tool_nodes.append(nav_tools) + + # Mapping + mapper_tools = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(package_dir, 'launch', 'robot_tools_launch.py') + ), + launch_arguments={ + 'rviz': use_rviz, + 'mapper': use_mapper, + 'use_sim_time': use_sim_time, + 'fill_map': fill_map, + }.items(), + condition=launch.conditions.IfCondition(use_mapper) + ) + tool_nodes.append(mapper_tools) + + # Wait for the simulation to be ready to start the tools + tools_handler = launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=diffdrive_controller_spawner, + on_exit=tool_nodes + ) + ) + return [ joint_state_broadcaster_spawner, diffdrive_controller_spawner, @@ -111,6 +156,7 @@ def get_ros2_nodes(*args): epuck_driver, footprint_publisher, epuck_process, + tools_handler, ] diff --git a/webots_ros2_epuck/launch/robot_tools_launch.py b/webots_ros2_epuck/launch/robot_tools_launch.py index 5a2231ae3..15dde6fa7 100644 --- a/webots_ros2_epuck/launch/robot_tools_launch.py +++ b/webots_ros2_epuck/launch/robot_tools_launch.py @@ -24,7 +24,6 @@ from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory, get_packages_with_prefixes from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.actions import ExecuteProcess def generate_launch_description(): @@ -36,7 +35,7 @@ def generate_launch_description(): use_rviz = LaunchConfiguration('rviz', default=True) use_mapper = LaunchConfiguration('mapper', default=False) fill_map = LaunchConfiguration('fill_map', default=True) - map_filename = LaunchConfiguration('fill_map', default=os.path.join(package_dir, 'resource', 'epuck_world_map.yaml')) + map_filename = LaunchConfiguration('map', default=os.path.join(package_dir, 'resource', 'epuck_world_map.yaml')) # Rviz node rviz_config = os.path.join(package_dir, 'resource', 'all.rviz') @@ -66,23 +65,6 @@ def generate_launch_description(): condition=launch.conditions.IfCondition(use_nav) ) ) - launch_description_nodes.append(ExecuteProcess( - cmd=[ - 'ros2', - 'topic', - 'pub', - '--once', - '/initialpose', - 'geometry_msgs/msg/PoseWithCovarianceStamped', - '{\ - "header": { "frame_id": "map" },\ - "pose": { "pose": {\ - "position": { "x": 0.005, "y": 0.0, "z": 0.0 },\ - "orientation": { "x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0 }}\ - }\ - }' - ] - )) else: launch_description_nodes.append(LogInfo(msg='Navigation2 is not installed, navigation functionality is disabled')) diff --git a/webots_ros2_epuck/launch/robot_with_tools_launch.py b/webots_ros2_epuck/launch/robot_with_tools_launch.py deleted file mode 100644 index d29a446d7..000000000 --- a/webots_ros2_epuck/launch/robot_with_tools_launch.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python - -# Copyright 1996-2023 Cyberbotics Ltd. -# -# 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. - -"""Launch Webots epuck driver and the additional tools.""" - -import os -from launch import LaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch.actions import IncludeLaunchDescription -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - package_dir = get_package_share_directory('webots_ros2_epuck') - - use_nav = LaunchConfiguration('nav', default=False) - use_rviz = LaunchConfiguration('rviz', default=False) - use_mapper = LaunchConfiguration('mapper', default=False) - synchronization = LaunchConfiguration('synchronization', default=False) - world = LaunchConfiguration('world', default='epuck_world.wbt') - - # Webots - webots_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(package_dir, 'launch', 'robot_launch.py') - ), - launch_arguments={ - 'synchronization': synchronization, - 'use_sim_time': 'true' - }.items() - ) - - # Base configuration - tools_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(package_dir, 'launch', 'robot_tools_launch.py') - ), - launch_arguments={ - 'nav': use_nav, - 'rviz': use_rviz, - 'mapper': use_mapper, - 'use_sim_time': 'true', - 'world': world - }.items() - ) - - return LaunchDescription([ - webots_launch, - tools_launch - ]) diff --git a/webots_ros2_epuck/resource/nav2_params.yaml b/webots_ros2_epuck/resource/nav2_params.yaml index 92521acd6..3a012ff68 100644 --- a/webots_ros2_epuck/resource/nav2_params.yaml +++ b/webots_ros2_epuck/resource/nav2_params.yaml @@ -1,15 +1,11 @@ amcl: ros__parameters: use_sim_time: True - alpha1: 0.0001 - alpha2: 0.0001 - alpha3: 0.0001 - alpha4: 0.0001 + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 alpha5: 0.2 - laser_sigma_hit: 0.2 - initial_cov_xx: 0.001 - initial_cov_yy: 0.001 - laser_likelihood_max_dist: 0.3 base_frame_id: "base_link" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 @@ -17,19 +13,20 @@ amcl: do_beamskip: false global_frame_id: "map" lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 laser_max_range: 100.0 laser_min_range: -1.0 laser_model_type: "likelihood_field" max_beams: 60 max_particles: 2000 - min_particles: 60 + 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: "differential" + robot_model_type: "nav2_amcl::DifferentialMotionModel" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true @@ -40,14 +37,12 @@ amcl: z_max: 0.05 z_rand: 0.5 z_short: 0.05 - -amcl_map_client: - ros__parameters: - use_sim_time: True - -amcl_rclcpp_node: - ros__parameters: - use_sim_time: True + scan_topic: scan + set_initial_pose: True + initial_pose: + x: 0.005 + y: 0.0 + yaw: 0.0 bt_navigator: ros__parameters: @@ -55,52 +50,110 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom - default_bt_xml_filename: "navigate_w_replanning_time.xml" + bt_loop_duration: 10 + default_server_timeout: 20 + # '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_back_up_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 + +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 - min_x_velocity_threshold: 0.0001 - min_y_velocity_threshold: 0.0001 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 - movement_time_allowance: 20.0 - required_movement_radius: 0.1 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.05 + yaw_goal_tolerance: 0.25 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 - max_vel_x: 0.04 + max_vel_x: 0.05 max_vel_y: 0.0 max_vel_theta: 1.0 min_speed_xy: 0.0 - max_speed_xy: 0.04 + max_speed_xy: 0.05 min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 acc_lim_x: 0.5 acc_lim_y: 0.0 acc_lim_theta: 3.2 @@ -114,7 +167,7 @@ controller_server: linear_granularity: 0.01 angular_granularity: 0.025 transform_tolerance: 1.5 - xy_goal_tolerance: 0.03 + xy_goal_tolerance: 0.05 trans_stopped_velocity: 0.04 short_circuit_trajectory_evaluation: True stateful: True @@ -133,14 +186,17 @@ controller_server: local_costmap: local_costmap: ros__parameters: - use_sim_time: True + update_frequency: 5.0 + publish_frequency: 2.0 global_frame: odom - plugins: ["voxel_layer", "inflation_layer"] + robot_base_frame: base_link + use_sim_time: True rolling_window: true width: 1 height: 1 resolution: 0.01 robot_radius: 0.035 + plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 @@ -161,23 +217,27 @@ local_costmap: clearing: True marking: True data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True always_send_full_costmap: True - local_costmap_client: - ros__parameters: - use_sim_time: True - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True global_costmap: global_costmap: ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link use_sim_time: True robot_radius: 0.035 + resolution: 0.05 + track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - inflation_layer: - plugin: nav2_costmap_2d::InflationLayer - inflation_radius: 0.20 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -188,18 +248,106 @@ global_costmap: clearing: True marking: True data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 always_send_full_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 - yaml_filename: "map.yaml" + # 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: "" + +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: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +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: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_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" + global_frame: odom + 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.26, 0.0, 1.0] + 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_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/webots_ros2_epuck/resource/nav2_rats_life_waypoints.yaml b/webots_ros2_epuck/resource/nav2_rats_life_waypoints.yaml index f9853af1a..ce3e7d823 100644 --- a/webots_ros2_epuck/resource/nav2_rats_life_waypoints.yaml +++ b/webots_ros2_epuck/resource/nav2_rats_life_waypoints.yaml @@ -7,25 +7,51 @@ bt_navigator: default_bt_xml_filename: "navigate_w_replanning_time.xml" 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_back_up_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 controller_server: ros__parameters: diff --git a/webots_ros2_epuck/resource/ros2_control.yml b/webots_ros2_epuck/resource/ros2_control.yml index 78dad84da..f1ad790df 100644 --- a/webots_ros2_epuck/resource/ros2_control.yml +++ b/webots_ros2_epuck/resource/ros2_control.yml @@ -13,8 +13,11 @@ diffdrive_controller: left_wheel_names: ["left wheel motor"] right_wheel_names: ["right wheel motor"] - wheel_separation: 0.05685 + wheel_separation: 0.052 wheel_radius: 0.02 + # The real separation between wheels is not resulting in a perfect odometry + wheel_separation_multiplier: 1.095 + use_stamped_vel: false base_frame_id: "base_link" diff --git a/webots_ros2_epuck/setup.py b/webots_ros2_epuck/setup.py index 31619b8a0..f5487041d 100644 --- a/webots_ros2_epuck/setup.py +++ b/webots_ros2_epuck/setup.py @@ -11,7 +11,6 @@ data_files.append(('share/' + package_name + '/launch', [ 'launch/robot_launch.py', 'launch/robot_tools_launch.py', - 'launch/robot_with_tools_launch.py', 'launch/rats_life_launch.py', 'launch/rats_life_waypoints_launch.py' ])) diff --git a/webots_ros2_tests/CHANGELOG.rst b/webots_ros2_tests/CHANGELOG.rst index 54c46f6ed..54c92fb9d 100644 --- a/webots_ros2_tests/CHANGELOG.rst +++ b/webots_ros2_tests/CHANGELOG.rst @@ -6,6 +6,7 @@ Changelog for package webots_ros2_tests ------------------ * Improved the TIAGo test by publishing the initial position when navigation is ready. * Fixed and improved Turtlebot navigation and mapping tests. +* Fixed and improved e-puck navigation test. 2022.1.4 (2022-11-18) ------------------ diff --git a/webots_ros2_tests/test/test_system_epuck_with_tools.py b/webots_ros2_tests/test/test_system_epuck_with_tools.py index d5e6b8395..ae91aa603 100644 --- a/webots_ros2_tests/test/test_system_epuck_with_tools.py +++ b/webots_ros2_tests/test/test_system_epuck_with_tools.py @@ -21,27 +21,29 @@ import os import pytest import rclpy -from nav_msgs.msg import OccupancyGrid +from nav_msgs.msg import OccupancyGrid, Odometry from launch import LaunchDescription import launch_testing.actions from ament_index_python.packages import get_package_share_directory from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.actions import IncludeLaunchDescription +from rclpy.action import ActionClient from webots_ros2_tests.utils import TestWebots, initialize_webots_test @pytest.mark.rostest def generate_test_description(): initialize_webots_test() + # If ROS_DISTRO is rolling, skip the test as some required packages are missing (cf. ci_after_init.bash) + # If ROS_DISTRO is foxy, skip the test as navigation parameters are not supported anymore + if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] != 'humble': + pytest.skip('ROS_DISTRO is rolling or foxy, skipping this test') epuck_with_tools_webots = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('webots_ros2_epuck'), 'launch', 'robot_with_tools_launch.py') + os.path.join(get_package_share_directory('webots_ros2_epuck'), 'launch', 'robot_launch.py') ), - launch_arguments={'nav': 'true', - 'rviz': 'true', - 'mapper': 'true', - 'synchronization': 'true'}.items(), + launch_arguments={'nav': 'true'}.items(), ) return LaunchDescription([ @@ -73,5 +75,28 @@ def on_map_message_received(message): self.wait_for_messages(self.__node, OccupancyGrid, '/map', condition=on_map_message_received) + def testMovement(self): + from nav2_msgs.action import NavigateToPose + + # Delay before publishing goal position (navigation initialization can be long in the CI) + goal_action = ActionClient(self.__node, NavigateToPose, 'navigate_to_pose') + goal_message = NavigateToPose.Goal() + goal_message.pose.header.stamp = self.__node.get_clock().now().to_msg() + goal_message.pose.header.frame_id = 'map' + goal_message.pose.pose.position.x = 0.37 + goal_message.pose.pose.position.y = 0.2 + goal_message.pose.pose.orientation.z = 0.544 + goal_message.pose.pose.orientation.w = 0.838 + goal_action.wait_for_server() + self.__node.get_logger().info('Server is ready, waiting 10 seconds to send goal position.') + self.wait_for_clock(self.__node, messages_to_receive=1000) + goal_action.send_goal_async(goal_message) + self.__node.get_logger().info('Goal position sent.') + + def on_message_received(message): + return message.pose.pose.position.x > 0.25 + + self.wait_for_messages(self.__node, Odometry, '/odom', condition=on_message_received, timeout=60*5) + def tearDown(self): self.__node.destroy_node() diff --git a/webots_ros2_tests/test/test_system_tiago.py b/webots_ros2_tests/test/test_system_tiago.py index 5f0754c59..cedbc4005 100644 --- a/webots_ros2_tests/test/test_system_tiago.py +++ b/webots_ros2_tests/test/test_system_tiago.py @@ -35,7 +35,7 @@ def generate_test_description(): initialize_webots_test() # If ROS_DISTRO is rolling, skip the test as some required packages are missing (cf. ci_after_init.bash) - # If ROS_DISTRO is foxy, skip the test as some navigation packages are outdated + # If ROS_DISTRO is foxy, skip the test as navigation parameters are not supported anymore if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] != 'humble': pytest.skip('ROS_DISTRO is rolling or foxy, skipping this test') diff --git a/webots_ros2_tests/test/test_system_turtlebot_tutorial_navigation.py b/webots_ros2_tests/test/test_system_turtlebot_tutorial_navigation.py index 79f1460ad..3eb342c72 100644 --- a/webots_ros2_tests/test/test_system_turtlebot_tutorial_navigation.py +++ b/webots_ros2_tests/test/test_system_turtlebot_tutorial_navigation.py @@ -36,7 +36,7 @@ def generate_test_description(): initialize_webots_test() # If ROS_DISTRO is rolling, skip the test as some required packages are missing (cf. ci_after_init.bash) - # If ROS_DISTRO is foxy, skip the test as some navigation packages are outdated + # If ROS_DISTRO is foxy, skip the test as navigation parameters are not supported anymore if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] != 'humble': pytest.skip('ROS_DISTRO is rolling or foxy, skipping this test') diff --git a/webots_ros2_tests/test/test_system_turtlebot_tutorial_slam.py b/webots_ros2_tests/test/test_system_turtlebot_tutorial_slam.py index 3b318b231..6179d7505 100644 --- a/webots_ros2_tests/test/test_system_turtlebot_tutorial_slam.py +++ b/webots_ros2_tests/test/test_system_turtlebot_tutorial_slam.py @@ -34,8 +34,9 @@ def generate_test_description(): initialize_webots_test() # If ROS_DISTRO is rolling, skip the test as some required packages are missing (cf. ci_after_init.bash) - if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling': - pytest.skip('ROS_DISTRO is rolling or humble, skipping this test') + # If ROS_DISTRO is foxy, skip the test as a random crash often occurs in testing + if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] != 'humble': + pytest.skip('ROS_DISTRO is rolling or foxy, skipping this test') # Webots turtlebot_webots = IncludeLaunchDescription(