diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 296898c..d286220 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -23,14 +23,13 @@ RUN apt-get update \ RUN mkdir -p /robocup/src/ WORKDIR /robocup -ADD https://raw.githubusercontent.com/mgonzs13/audio_common/refs/heads/main/requirements.txt /robocup/requirements1.txt -ADD https://raw.githubusercontent.com/mgonzs13/whisper_ros/refs/heads/main/requirements.txt /robocup/requirements2.txt -ADD https://raw.githubusercontent.com/mgonzs13/llama_ros/refs/heads/main/requirements.txt /robocup/requirements3.txt -ADD https://raw.githubusercontent.com/jmguerreroh/yolov8_ros/refs/heads/main/requirements.txt /robocup/requirements4.txt -ADD https://raw.githubusercontent.com/mgonzs13/tts_ros/refs/heads/main/requirements.txt /robocup/requirements5.txt +ADD https://raw.githubusercontent.com/mgonzs13/audio_common/refs/heads/main/requirements.txt /robocup/requirements2.txt +ADD https://raw.githubusercontent.com/mgonzs13/whisper_ros/refs/heads/main/requirements.txt /robocup/requirements3.txt +ADD https://raw.githubusercontent.com/mgonzs13/llama_ros/refs/heads/main/requirements.txt /robocup/requirements4.txt +ADD https://raw.githubusercontent.com/jmguerreroh/yolov8_ros/refs/heads/main/requirements.txt /robocup/requirements5.txt # Install external dependencies. -RUN pip install -r requirements1.txt --break-system-packages +RUN pip install coqui-tts --break-system-packages RUN pip install -r requirements2.txt --break-system-packages RUN pip install -r requirements3.txt --break-system-packages RUN pip install -r requirements4.txt --break-system-packages diff --git a/README.md b/README.md index 5987504..73193b3 100644 --- a/README.md +++ b/README.md @@ -2,14 +2,40 @@ ## Installation -See [Software setup](https://github.com/CoreSenseEU/CoreSense4Home/wiki/C-Software-Setup) +### Clone the repos: +```bash +$ mkdir -p robocup24_ws/src +$ cd robocup24_ws/src +robocup24_ws/src $ git clone https://github.com/CoreSenseEU/CoreSense4Home.git -b jazzy +``` +### Install dependencies +```bash +robocup24_ws/src $ vcs import --recursive < CoreSense4Home/robocup_bringup/thirdparty.repos +robocup24_ws/src $ cd .. +robocup24_ws $ rosdep install --from-paths src --ignore-src -r -y +``` -## Usage -### Navigation +### Install Python dependencies +If you have exeperience working using venv with python please use them. Otherwise: ```bash -ros2 launch robocup_bringup navigation.launch.py +cd robocup24_ws +pip install -r src/CoreSense4Home/requirements.txt --break-system-packages +pip install -r src/ThirdParty/whisper_ros/requirements.txt --break-system-packages +pip install -r src/ThirdParty/llama_ros/requirements.txt --break-system-packages +pip install -r src/ThirdParty/yolov8_ros/requirements.txt --break-system-packages +pip install -r src/ThirdParty/audio_common/requirements.txt --break-system-packages ``` +### Compile +``` bash +cd robocup24_ws +colcon build --cmake-args -DGGML_CUDA=ON +``` + +## Usage + +Dont forget to put the flag use_sim_time:=False + ### Launch current carry my luggage implementation First kill move_group node inside tiago robot. Then in separate terminals launch: diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..a33c4bc --- /dev/null +++ b/requirements.txt @@ -0,0 +1 @@ +coqui-tts \ No newline at end of file diff --git a/robocup_bringup/CMakeLists.txt b/robocup_bringup/CMakeLists.txt index 619196a..e9d6a2f 100644 --- a/robocup_bringup/CMakeLists.txt +++ b/robocup_bringup/CMakeLists.txt @@ -15,7 +15,6 @@ find_package(motion REQUIRED) find_package(perception REQUIRED) find_package(attention_system REQUIRED) find_package(perception_system REQUIRED) -find_package(nav2_behavior_tree REQUIRED) find_package(backward_ros REQUIRED) set(CMAKE_CXX_STANDARD 17) @@ -31,7 +30,6 @@ set(dependencies perception attention_system perception_system - nav2_behavior_tree backward_ros ) diff --git a/robocup_bringup/bt_xml/receptionist.xml b/robocup_bringup/bt_xml/receptionist.xml index f8d86fa..b0d2a3e 100644 --- a/robocup_bringup/bt_xml/receptionist.xml +++ b/robocup_bringup/bt_xml/receptionist.xml @@ -43,13 +43,7 @@ - - - - - + @@ -141,7 +135,6 @@ + guest_color_id_2="{guest_color_id_2}"/> @@ -217,7 +208,6 @@ - @@ -255,8 +245,7 @@ - - + @@ -303,12 +292,12 @@ - + high_z="0.9"/> --> - + diff --git a/robocup_bringup/config/receptionist/receptionist_params.yaml b/robocup_bringup/config/receptionist/receptionist_params.yaml index 1f9fbef..9e93e99 100644 --- a/robocup_bringup/config/receptionist/receptionist_params.yaml +++ b/robocup_bringup/config/receptionist/receptionist_params.yaml @@ -14,9 +14,9 @@ # guest_confirmation: [15.103, -8.220, -1.949] # ARENA B: waypoints: # [x, y, yaw] only 3 numbers!! - entrance: [-11.886, 2.915, 1.952] - party: [-13.452, -1.870, -2.39] - guest_confirmation: [-13.452, -1.870, 2.500] + entrance: [0.676, -4.427, 2.142] + party: [2.048, -5.675, 0.604] + guest_confirmation: [2.048, -5.675, 2.170] behaviors_main: ros__parameters: diff --git a/robocup_bringup/config/receptionist/tiago_nav_params.yaml b/robocup_bringup/config/receptionist/tiago_nav_params.yaml index 763a8ea..46e87e7 100644 --- a/robocup_bringup/config/receptionist/tiago_nav_params.yaml +++ b/robocup_bringup/config/receptionist/tiago_nav_params.yaml @@ -1,26 +1,25 @@ amcl: ros__parameters: - use_sim_time: False - alpha1: 0.1 - alpha2: 0.1 - alpha3: 0.1 - alpha4: 0.1 - alpha5: 0.1 - base_frame_id: "base_footprint" + use_sim_time: true + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + base_frame_id: base_footprint beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 do_beamskip: false - global_frame_id: "map" + global_frame_id: map lambda_short: 0.1 laser_likelihood_max_dist: 2.0 - laser_max_range: 5.0 + laser_max_range: 100.0 laser_min_range: -1.0 laser_model_type: "likelihood_field" max_beams: 60 max_particles: 2000 min_particles: 500 - odom_frame_id: "odom" + odom_frame_id: odom pf_err: 0.05 pf_z: 0.99 recovery_alpha_fast: 0.0 @@ -40,112 +39,119 @@ amcl: scan_topic: /scan_raw set_initial_pose: true initial_pose: - x: 2.0 - y: 3.6 - yaw: 3.14 - # receptionist initial_pose: - # initial_pose: - # x: 0.99 - # y: 0.283 - # z: 0.0 - # yaw: -1.551 + x: 0.0 + y: 0.0 + z: 0.0 + yaw: 0.0 - # inspection iniutial_pose: - # initial_pose: - # x: -2.700 - # y: -0.641 - # yaw: 0.0 - # initial_pose: - # x: 0.5156184016639794 - # y: 1.2985416281620732 - # z: 0.0 - # yaw: 1.6028087 +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 + 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 + enable_stamped_cmd_vel: false - # storing initial_pose: - # initial_pose: - # x: -2.315 - # y: -0.646 - # z: 0.0 - # yaw: -2.231 - # # carry initial_pose - bt_navigator: ros__parameters: - use_sim_time: False global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + 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_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_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_drive_on_heading_cancel_bt_node - # - clear_map_at_goal_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + + error_code_names: + - compute_path_error_code + - follow_path_error_code bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: - use_sim_time: False + use_sim_time: true bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: - use_sim_time: False + use_sim_time: true + +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_raw" + min_height: 0.15 + max_height: 2.0 + enabled: True controller_server: ros__parameters: - use_sim_time: False + use_sim_time: true controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 + enable_stamped_cmd_vel: false progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + goal_checker_plugins: ["general_goal_checker"] controller_plugins: ["FollowPath"] + odom_topic: /mobile_base_controller/odom # Progress checker parameters progress_checker: @@ -153,30 +159,23 @@ controller_server: 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.25 yaw_goal_tolerance: 0.25 + stateful: True # 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.6 + max_vel_x: 1.00 max_vel_y: 0.0 - max_vel_theta: 0.6 + max_vel_theta: 5.00 min_speed_xy: 0.0 - max_speed_xy: 0.6 + max_speed_xy: 1.00 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: 2.5 acc_lim_y: 0.0 acc_lim_theta: 3.2 @@ -206,33 +205,67 @@ controller_server: RotateToGoal.slowing_factor: 5.0 RotateToGoal.lookahead_time: -1.0 -local_costmap: - local_costmap: +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + 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 + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + +global_costmap: + global_costmap: ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map robot_base_frame: base_link - use_sim_time: False - rolling_window: true - width: 3 - height: 3 + use_sim_time: true + robot_radius: 0.275 resolution: 0.05 - robot_radius: 0.29 - plugins: ["obstacle_layer" ,"inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.45 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 observation_sources: scan scan: topic: /scan_raw @@ -247,41 +280,39 @@ local_costmap: static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True - clear_people_layer: - plugin: "nav2_costmap_2d::ClearPeopleLayer" - person_radious: 1.0 - person_frame: person_0_filtered - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan_raw - max_obstacle_height: 2.0 - 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 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 always_send_full_costmap: True -global_costmap: - global_costmap: +local_costmap: + local_costmap: ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom robot_base_frame: base_link - use_sim_time: False - robot_radius: 0.29 + use_sim_time: true + rolling_window: true + width: 3 + height: 3 resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" + robot_radius: 0.275 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 observation_sources: scan scan: topic: /scan_raw @@ -296,26 +327,11 @@ global_costmap: 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.40 - clear_people_layer: - plugin: "nav2_costmap_2d::ClearPeopleLayer" - person_radious: 1.0 - person_frame: person_0_filtered - enabled: True - always_send_full_costmap: True -map_server: - ros__parameters: - use_sim_time: False - yaml_filename: "lab.yaml" - map_saver: ros__parameters: - use_sim_time: False + use_sim_time: true save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 @@ -324,133 +340,108 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: False + use_sim_time: true planner_plugins: ["GridBased"] GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" + plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true -smoother_server: +slam_toolbox: ros__parameters: - use_sim_time: False - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True + use_sim_time: true + # 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 -behavior_server: + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan_raw + mode: mapping + + # 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: 30.0 + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # 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 + +smoother_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", "wait"] - spin: - plugin: "nav2_behaviors/Spin" - backup: - plugin: "nav2_behaviors/BackUp" - drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" - wait: - plugin: "nav2_behaviors/Wait" - global_frame: odom - robot_base_frame: base_link - transform_tolerance: 0.1 - use_sim_time: False - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 + use_sim_time: true -robot_state_publisher: +velocity_smoother: ros__parameters: - use_sim_time: False + use_sim_time: true + enable_stamped_cmd_vel: false waypoint_follower: ros__parameters: - use_sim_time: False - loop_rate: 20 + use_sim_time: true 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 + waypoint_pause_duration: 0.0 -velocity_smoother: +map_server: ros__parameters: use_sim_time: False - 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: "/mobile_base_controller/odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 - -slam_toolbox: - ros__parameters: - angle_variance_penalty: 1.0 - base_frame: base_footprint - ceres_dogleg_type: TRADITIONAL_DOGLEG - ceres_linear_solver: SPARSE_NORMAL_CHOLESKY - ceres_loss_function: None - ceres_preconditioner: SCHUR_JACOBI - ceres_trust_strategy: LEVENBERG_MARQUARDT - coarse_angle_resolution: 0.0349 - coarse_search_angle_offset: 0.349 - correlation_search_space_dimension: 0.5 - correlation_search_space_resolution: 0.01 - correlation_search_space_smear_deviation: 0.1 - debug_logging: false - distance_variance_penalty: 0.5 - do_loop_closing: true - enable_interactive_mode: true - fine_search_angle_offset: 0.00349 - link_match_minimum_response_fine: 0.1 - link_scan_maximum_distance: 1.5 - loop_match_maximum_variance_coarse: 3.0 - loop_match_minimum_chain_size: 10 - loop_match_minimum_response_coarse: 0.35 - loop_match_minimum_response_fine: 0.45 - loop_search_maximum_distance: 3.0 - loop_search_space_dimension: 8.0 - loop_search_space_resolution: 0.05 - loop_search_space_smear_deviation: 0.03 - # map_file_name: /home/juan/workspaces/robocup/install/robocup_bringup/share/robocup_bringup/maps/lab_robocup - # map_frame: map - map_start_pose: # # - 2.0 -# # - 3.6 -# # - 3.1416 - - -2.0 - - 3.6 - - 3.1416 - map_update_interval: 0.1 - max_laser_range: 20.0 - minimum_angle_penalty: 0.9 - minimum_distance_penalty: 0.5 - minimum_time_interval: 0.1 - minimum_travel_distance: 0.01 - minimum_travel_heading: 0.01 - mode: localization - odom_frame: odom - resolution: 0.05 - scan_buffer_maximum_scan_distance: 10.0 - scan_buffer_size: 10 - scan_topic: /scan_raw - solver_plugin: solver_plugins::CeresSolver - stack_size_to_use: 40000000 - tf_buffer_duration: 30.0 - throttle_scans: 1 - transform_publish_period: 0.02 - transform_timeout: 0.2 - use_response_expansion: true - use_scan_barycenter: true - use_scan_matching: true \ No newline at end of file + yaml_filename: "security_map.yaml" \ No newline at end of file diff --git a/robocup_bringup/launch/dialog.launch.py b/robocup_bringup/launch/dialog.launch.py index dae5514..bddad70 100644 --- a/robocup_bringup/launch/dialog.launch.py +++ b/robocup_bringup/launch/dialog.launch.py @@ -78,7 +78,7 @@ def generate_launch_description(): parameters=[ {'chunk': 4096}, {'frame_id': ''}, - {'model': 'tts_models/en/ljspeech/vits'}, + {'model': 'tts_models/en/ljspeech/glow-tts'}, {'speaker_wav': ''}, {'device': 'cuda'}] ) diff --git a/robocup_bringup/launch/receptionist_dependencies.launch.py b/robocup_bringup/launch/receptionist_dependencies.launch.py index d3c5abe..a070d05 100644 --- a/robocup_bringup/launch/receptionist_dependencies.launch.py +++ b/robocup_bringup/launch/receptionist_dependencies.launch.py @@ -63,7 +63,7 @@ def generate_launch_description(): 'input_depth_topic': '/head_front_camera/depth/image_raw', 'input_depth_info_topic': '/head_front_camera/depth/camera_info', 'depth_image_units_divisor': '1000', # 1 for simulation, 1000 real - 'target_frame': 'head_front_camera_link_color_optical_frame', + 'target_frame': 'head_front_camera_rgb_optical_frame', 'threshold': '0.5' }.items() ) @@ -80,8 +80,7 @@ def generate_launch_description(): ), launch_arguments={ 'rviz': 'True', - # 'map': package_dir + '/maps/robocup_arena_1.yaml', # ARENA C - 'map': package_dir + '/maps/robocup_arena_2.yaml', # ARENA B + 'map': package_dir + '/maps/security_map.yaml', 'params_file': package_dir + '/config/receptionist/tiago_nav_params.yaml', 'slam_params_file': package_dir + @@ -95,7 +94,7 @@ def generate_launch_description(): ld.add_action(dialog) ld.add_action(yolo3d) ld.add_action(real_time) - ld.add_action(move_group) - ld.add_action(manipulation_server) + # ld.add_action(move_group) + # ld.add_action(manipulation_server) return ld diff --git a/robocup_bringup/maps/security_map.pgm b/robocup_bringup/maps/security_map.pgm new file mode 100644 index 0000000..6f762a1 Binary files /dev/null and b/robocup_bringup/maps/security_map.pgm differ diff --git a/robocup_bringup/maps/security_map.yaml b/robocup_bringup/maps/security_map.yaml new file mode 100644 index 0000000..026bd25 --- /dev/null +++ b/robocup_bringup/maps/security_map.yaml @@ -0,0 +1,7 @@ +image: security_map.pgm +mode: trinary +resolution: 0.05 +origin: [-5.96, -11.8, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/robocup_bringup/package.xml b/robocup_bringup/package.xml index 2f5120a..95ed5ea 100644 --- a/robocup_bringup/package.xml +++ b/robocup_bringup/package.xml @@ -18,6 +18,7 @@ hri motion perception + launch_pal ament_lint_auto ament_lint_common diff --git a/robocup_bringup/thirdparty.repos b/robocup_bringup/thirdparty.repos index d8e09c4..9ce2c68 100644 --- a/robocup_bringup/thirdparty.repos +++ b/robocup_bringup/thirdparty.repos @@ -71,7 +71,42 @@ repositories: type: git url: https://github.com/igonzf/GPSR_planning.git version: main - + ThirdParty/launch_pal: + type: git + url: https://github.com/Tiago-Harmonic/launch_pal.git + version: master + ThirdParty/play_motion2: + type: git + url: https://github.com/Tiago-Harmonic/play_motion2.git + version: jazzy + ThirdParty/tiago_moveit_config: + type: git + url: https://github.com/Tiago-Harmonic/tiago_moveit_config.git + version: jazzy + ThirdParty/tiago_omni_base: + type: git + url: https://github.com/Tiago-Harmonic/omni_base_robot.git + version: jazzy + ThirdParty/pal_urdf_utils: + type: git + url: https://github.com/pal-robotics/pal_urdf_utils.git + version: humble-devel + ThirdParty/pmb2_robot: + type: git + url: https://github.com/Tiago-Harmonic/pmb2_robot.git + version: jazzy + ThirdParty/pal_hey5: + type: git + url: https://github.com/Tiago-Harmonic/pal_hey5.git + version: jazzy + ThirdParty/pal_gripper: + type: git + url: https://github.com/Tiago-Harmonic/pal_gripper.git + version: jazzy + ThirdParty/pal_robotiq_gripper: + type: git + url: https://github.com/Tiago-Harmonic/pal_robotiq_gripper.git + version: jazzy