zbw/yiliao2026
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

a

Browse Source
This commit is contained in:
mohuanzui
2026-06-02 12:55:53 +08:00
commit 9fd2426ac5
5188 changed files with 215542 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
src/gc_bringup/CMakeLists.txt Normal file
View File

@@ -0,0 +1,29 @@
cmake_minimum_required(VERSION 3.5)
project(gc_bringup)
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
find_package(ament_cmake REQUIRED)
install(
DIRECTORY launch config maps rviz
DESTINATION share/${PROJECT_NAME}
)
install(PROGRAMS
scripts/nav_goal.py
DESTINATION lib/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

77
src/gc_bringup/README.md Normal file
View File

@@ -0,0 +1,77 @@
# gc_bringup
GC 机器人启动包 — 阿克曼底盘 + 激光雷达 + SLAM 建图 + AMCL 导航
## 📁 目录结构
```
gc_bringup/
├── launch/
│ ├── slam_mapping.launch.py # SLAM 在线建图
│ ├── navigation.launch.py # AMCL 定位 + Nav2 导航
│ └── navigation_slam.launch.py # slam_toolbox 定位 + Nav2 导航
├── config/
│ ├── nav2_params.yaml # Nav2 参数（阿克曼配置）
│ ├── mapper_params_online_async.yaml # slam_toolbox 建图参数
│ └── mapper_params_localization.yaml # slam_toolbox 定位参数
├── maps/ # 地图文件存放
├── rviz/ # RViz2 配置文件
├── CMakeLists.txt
└── package.xml
```
## 🚀 使用方式
### 1. 编译
```bash
cd ~/test_ws
colcon build --packages-select gc_bringup
source install/setup.bash
```
### 2. SLAM 建图
启动 Gazebo + 机器人 + slam_toolbox 在线异步建图：
```bash
ros2 launch gc_bringup slam_mapping.launch.py
```
在 RViz 中使用 "2D Nav Goal" 工具驱动机器人探索环境建图。
保存地图：
```bash
ros2 run nav2_map_server map_saver_cli -f ~/test_ws/src/gc_bringup/maps/my_map
```
### 3. AMCL 导航（推荐）
使用建好的地图进行 AMCL 定位 + Nav2 导航：
```bash
ros2 launch gc_bringup navigation.launch.py
```
操作步骤：
1. 在 RViz 中使用 "2D Pose Estimate" 设置初始位姿
2. 使用 "2D Nav Goal" 设置导航目标
### 4. slam_toolbox 定位导航
使用 slam_toolbox 定位模式（需要 .posegraph 文件）：
```bash
ros2 launch gc_bringup navigation_slam.launch.py
```
## 🔧 启动参数
| 参数 | 默认值 | 说明 |
|------|--------|------|
| `use_sim_time` | `true` | 使用仿真时间 |
| `use_gazebo` | `true` | 启动 Gazebo |
| `use_rviz` | `true` | 启动 RViz2 |
| `world` | `origincar_description/world/gc_world.world` | Gazebo 世界文件 |
| `map` | `gc_bringup/maps/my_map.yaml` | 导航用地图 |
## 🤖 机器人说明
- **底盘类型**: 阿克曼转向 (Ackermann)
- **传感器**: 360° 激光雷达 (LiDAR)、IMU
- **控制器**: MPPI (Ackermann 运动模型)
- **规划器**: Smac Hybrid (Reeds-Shepp，支持倒车)
- **定位**: AMCL (自适应蒙特卡洛定位) 或 slam_toolbox 定位模式

60
src/gc_bringup/config/mapper_params_localization.yaml Normal file
View File

@@ -0,0 +1,60 @@
# =============================================================================
# slam_toolbox 定位模式参数
# 用于在已有地图上进行定位（替代 AMCL）
# =============================================================================
slam_toolbox:
ros__parameters:
# 基本设置
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 参数
odom_frame: odom
map_frame: map
base_frame: base_footprint
scan_topic: /scan
use_sim_time: True
mode: localization
# 调试
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02
map_update_interval: 10.0
resolution: 0.05
max_laser_range: 3.5
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.0
stack_size_to_use: 40000000
# 定位模式不需要回环检测
loop_search_maximum_distance: 3.0
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_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# 回环闭合
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# 扫描匹配器
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

61
src/gc_bringup/config/mapper_params_online_async.yaml Normal file
View File

@@ -0,0 +1,61 @@
# =============================================================================
# slam_toolbox 在线异步建图参数
# 用于实时 SLAM 建图
# =============================================================================
slam_toolbox:
ros__parameters:
# 基本设置
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 参数
odom_frame: odom
map_frame: map
base_frame: base_footprint
scan_topic: /scan
use_sim_time: True
mode: mapping
# 调试
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02
map_update_interval: 5.0
resolution: 0.05
max_laser_range: 3.5
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.0
stack_size_to_use: 40000000
enable_interactive_mode: true
# 回环检测
loop_search_maximum_distance: 3.0
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_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# 回环闭合
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# 扫描匹配器
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

411
src/gc_bringup/config/nav2_params.yaml Normal file
View File

@@ -0,0 +1,411 @@
# =============================================================================
# Nav2 参数 — 阿克曼底盘 + AMCL + MPPI
# =============================================================================
amcl:
ros__parameters:
use_sim_time: True
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 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"
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: 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
# ★ 自动设定初始位姿 — 不设置的话 map→odom 的 TF 不会发布
set_initial_pose: true
initial_pose:
x: 0.0
y: 0.0
z: 0.0
yaw: 0.0
robot_model_type: "nav2_amcl::DifferentialMotionModel"
save_pose_rate: 0.5
sigma_hit: 0.2
tf_broadcast: true
transform_tolerance: 10.0
update_min_a: 0.2
update_min_d: 0.25
z_hit: 0.5
z_max: 0.05
z_rand: 0.5
z_short: 0.05
scan_topic: scan
amcl_map_client:
ros__parameters:
use_sim_time: True
amcl_rclcpp_node:
ros__parameters:
use_sim_time: True
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_footprint
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
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
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 30.0
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
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
vy_std: 0.0
wz_std: 0.4
vx_max: 0.5
vx_min: -0.35
vy_max: 0.0
wz_max: 1.9
iteration_count: 1
temperature: 0.3
gamma: 0.015
motion_model: "Ackermann"
visualize: false
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstraints:
min_turning_r: 0.4
critics:
- "ConstraintCritic"
- "CostCritic"
- "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
CostCritic:
enabled: true
cost_power: 1
cost_weight: 3.81
critical_cost: 300.0
consider_footprint: true
collision_cost: 1000000.0
near_goal_distance: 1.0
trajectory_point_step: 2
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 4
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
forward_preference: true
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
use_sim_time: True
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
downsample_costmap: false
downsampling_factor: 1
tolerance: 0.25
allow_unknown: true
max_iterations: 1000000
max_on_approach_iterations: 1000
max_planning_time: 5.0
motion_model_for_search: "REEDS_SHEPP"
angle_quantization_bins: 72
analytic_expansion_ratio: 3.5
analytic_expansion_max_length: 3.0
minimum_turning_radius: 0.40
reverse_penalty: 2.0
change_penalty: 0.0
non_straight_penalty: 1.2
cost_penalty: 2.0
retrospective_penalty: 0.015
lookup_table_size: 20.0
cache_obstacle_heuristic: false
viz_expansions: false
smooth_path: True
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1.0e-10
do_refinement: true
refinement_num: 2
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: 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", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
plugin: "nav2_behaviors/BackUp"
wait:
plugin: "nav2_behaviors/Wait"
global_frame: odom
robot_base_frame: base_footprint
transform_tolerance: 0.5
use_sim_time: True
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
behavior_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
transform_tolerance: 0.5
global_frame: odom
robot_base_frame: base_footprint
use_sim_time: True
rolling_window: true
width: 3
height: 3
resolution: 0.05
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
footprint_padding: 0.02
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
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
static_layer:
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:
use_sim_time: True
transform_tolerance: 0.5
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_footprint
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
footprint_padding: 0.02
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /scan
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
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_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
robot_state_publisher:
ros__parameters:
use_sim_time: True
waypoint_follower:
ros__parameters:
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

200
src/gc_bringup/launch/navigation.launch.py Normal file
View File

@@ -0,0 +1,200 @@
#!/usr/bin/env python3
# =============================================================================
# gc_bringup — AMCL 导航模式 (直接启动 Nav2 节点)
# 启动 Gazebo + 机器人 + Nav2 (AMCL 定位)
# =============================================================================
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, ExecuteProcess,
TimerAction)
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
def generate_launch_description():
bringup_dir = get_package_share_directory('gc_bringup')
origincar_dir = get_package_share_directory('origincar_description')
# ==================== 参数 ====================
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
use_gazebo = LaunchConfiguration('use_gazebo', default='true')
use_rviz = LaunchConfiguration('use_rviz', default='false')
world = LaunchConfiguration('world', default=os.path.join(
origincar_dir, 'world', 'gc_world.world'))
map_yaml = LaunchConfiguration('map', default=os.path.join(
bringup_dir, 'maps', 'my_map.yaml'))
declare_use_sim_time = DeclareLaunchArgument('use_sim_time', default_value='true')
declare_use_gazebo = DeclareLaunchArgument('use_gazebo', default_value='true')
declare_use_rviz = DeclareLaunchArgument('use_rviz', default_value='false')
declare_world = DeclareLaunchArgument(
'world', default_value=os.path.join(origincar_dir, 'world', 'gc_world.world'))
declare_map = DeclareLaunchArgument(
'map', default_value=os.path.join(bringup_dir, 'maps', 'my_map.yaml'))
# ==================== 机器人模型 ====================
default_model_path = os.path.join(origincar_dir, 'urdf', 'origincar.urdf')
robot_description = ParameterValue(
Command(['xacro ', default_model_path]), value_type=str)
# ==================== Gazebo ====================
start_gazebo = ExecuteProcess(
condition=IfCondition(use_gazebo),
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
'-s', 'libgazebo_ros_factory.so', world],
output='screen')
spawn_robot = Node(
condition=IfCondition(use_gazebo),
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', 'mycar', '-file', default_model_path,
'-x', '0.0', '-y', '0.0', '-z', '0.1'],
output='screen')
# ==================== TF & 机器人状态 ====================
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[{'robot_description': robot_description,
'use_sim_time': True, 'publish_frequency': 30.0}],
output='screen')
joint_state_publisher = Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
parameters=[{'use_sim_time': True}], output='screen')
# ==================== Nav2 参数文件路径 ====================
nav2_params = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
# ==================== Map Server ====================
map_server = Node(
package='nav2_map_server',
executable='map_server',
name='map_server',
output='screen',
parameters=[nav2_params,
{'use_sim_time': True, 'yaml_filename': map_yaml}],
)
# ==================== AMCL ====================
amcl = Node(
package='nav2_amcl',
executable='amcl',
name='amcl',
output='screen',
parameters=[nav2_params, {'use_sim_time': True}],
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
)
# ==================== Planner Server ====================
planner_server = Node(
package='nav2_planner',
executable='planner_server',
name='planner_server',
output='screen',
parameters=[nav2_params, {'use_sim_time': True}],
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
)
# ==================== Controller Server ====================
controller_server = Node(
package='nav2_controller',
executable='controller_server',
name='controller_server',
output='screen',
parameters=[nav2_params, {'use_sim_time': True}],
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
)
# ==================== Behavior Server ====================
behavior_server = Node(
package='nav2_behaviors',
executable='behavior_server',
name='behavior_server',
output='screen',
parameters=[nav2_params, {'use_sim_time': True}],
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
)
# ==================== BT Navigator ====================
bt_navigator = Node(
package='nav2_bt_navigator',
executable='bt_navigator',
name='bt_navigator',
output='screen',
parameters=[nav2_params, {'use_sim_time': True}],
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
)
# ==================== Waypoint Follower ====================
waypoint_follower = Node(
package='nav2_waypoint_follower',
executable='waypoint_follower',
name='waypoint_follower',
output='screen',
parameters=[nav2_params, {'use_sim_time': True}],
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
)
# ==================== Lifecycle Manager (map + amcl) ====================
lifecycle_localization = Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_localization',
output='screen',
parameters=[{'use_sim_time': True, 'autostart': True,
'node_names': ['map_server', 'amcl']}],
)
# ==================== Lifecycle Manager (nav nodes) ====================
lifecycle_navigation = Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_navigation',
output='screen',
parameters=[{'use_sim_time': True, 'autostart': True,
'node_names': ['controller_server', 'planner_server',
'behavior_server', 'bt_navigator',
'waypoint_follower']}],
)
# ==================== RViz2 (可选) ====================
rviz_config = os.path.join(bringup_dir, 'rviz', 'navigation.rviz')
rviz_node = Node(
condition=IfCondition(use_rviz),
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config],
parameters=[{'use_sim_time': True}],
output='screen')
# ==================== 组装 ====================
ld = LaunchDescription()
ld.add_action(declare_use_sim_time)
ld.add_action(declare_use_gazebo)
ld.add_action(declare_use_rviz)
ld.add_action(declare_world)
ld.add_action(declare_map)
ld.add_action(start_gazebo)
ld.add_action(TimerAction(period=5.0, actions=[spawn_robot]))
ld.add_action(robot_state_publisher)
ld.add_action(joint_state_publisher)
ld.add_action(TimerAction(period=8.0, actions=[
map_server, amcl,
planner_server, controller_server, behavior_server, bt_navigator,
waypoint_follower,
lifecycle_localization, lifecycle_navigation,
rviz_node,
]))
return ld

150
src/gc_bringup/launch/navigation_slam.launch.py Normal file
View File

@@ -0,0 +1,150 @@
#!/usr/bin/env python3
# =============================================================================
# gc_bringup — slam_toolbox 定位 + 导航模式
# 使用 slam_toolbox 定位模式替代 AMCL，适用于有 posegraph 的场景
# =============================================================================
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, ExecuteProcess,
IncludeLaunchDescription, TimerAction)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
def generate_launch_description():
# ============================ 包路径 ============================
bringup_dir = get_package_share_directory('gc_bringup')
origincar_dir = get_package_share_directory('origincar_description')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
# ============================ 启动参数 ============================
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
use_gazebo = LaunchConfiguration('use_gazebo', default='true')
use_rviz = LaunchConfiguration('use_rviz', default='true')
world = LaunchConfiguration('world', default=os.path.join(
origincar_dir, 'world', 'gc_world.world'))
map_yaml = LaunchConfiguration('map', default=os.path.join(
bringup_dir, 'maps', 'my_map.yaml'))
declare_use_sim_time = DeclareLaunchArgument('use_sim_time', default_value='true')
declare_use_gazebo = DeclareLaunchArgument('use_gazebo', default_value='true')
declare_use_rviz = DeclareLaunchArgument('use_rviz', default_value='true')
declare_world = DeclareLaunchArgument(
'world', default_value=os.path.join(origincar_dir, 'world', 'gc_world.world'))
declare_map = DeclareLaunchArgument(
'map', default_value=os.path.join(bringup_dir, 'maps', 'my_map.yaml'))
# ============================ 机器人模型 ============================
default_model_path = os.path.join(origincar_dir, 'urdf', 'origincar.urdf')
robot_description = ParameterValue(
Command(['xacro ', default_model_path]), value_type=str)
# ============================ Gazebo ============================
start_gazebo = ExecuteProcess(
condition=IfCondition(use_gazebo),
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
'-s', 'libgazebo_ros_factory.so', world],
output='screen'
)
spawn_robot = Node(
condition=IfCondition(use_gazebo),
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', 'mycar',
'-file', default_model_path,
'-x', '0.0', '-y', '0.0', '-z', '0.1'],
output='screen'
)
# ============================ TF & 机器人状态 ============================
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[{
'robot_description': robot_description,
'use_sim_time': True,
'publish_frequency': 30.0
}],
output='screen'
)
joint_state_publisher = Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
parameters=[{'use_sim_time': True}],
output='screen'
)
# ============================ slam_toolbox 定位模式 ============================
map_base_path = os.path.join(bringup_dir, 'maps', 'my_map')
slam_localization_params = os.path.join(
bringup_dir, 'config', 'mapper_params_localization.yaml')
slam_toolbox_node = Node(
package='slam_toolbox',
executable='localization_slam_toolbox_node',
name='slam_toolbox',
output='screen',
parameters=[
slam_localization_params,
{
'map_file_name': map_base_path,
'use_sim_time': True
}
]
)
# ============================ Nav2 Bringup (关闭内部定位，用 slam_toolbox) ============================
nav2_params = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
nav2_launch_path = os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
nav2_bringup = IncludeLaunchDescription(
PythonLaunchDescriptionSource(nav2_launch_path),
launch_arguments={
'map': map_yaml,
'use_sim_time': use_sim_time,
'params_file': nav2_params,
'slam': 'False',
'autostart': 'True',
'use_composition': 'False',
}.items(),
)
# ============================ RViz2 ============================
rviz_config = os.path.join(bringup_dir, 'rviz', 'navigation.rviz')
rviz_node = Node(
condition=IfCondition(use_rviz),
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config],
parameters=[{'use_sim_time': True}],
output='screen'
)
# ============================ 组装 ============================
ld = LaunchDescription()
ld.add_action(declare_use_sim_time)
ld.add_action(declare_use_gazebo)
ld.add_action(declare_use_rviz)
ld.add_action(declare_world)
ld.add_action(declare_map)
ld.add_action(start_gazebo)
ld.add_action(TimerAction(period=5.0, actions=[spawn_robot]))
ld.add_action(robot_state_publisher)
ld.add_action(joint_state_publisher)
ld.add_action(TimerAction(
period=8.0,
actions=[slam_toolbox_node, nav2_bringup, rviz_node]
))
return ld

153
src/gc_bringup/launch/slam_mapping.launch.py Normal file
View File

@@ -0,0 +1,153 @@
#!/usr/bin/env python3
# =============================================================================
# gc_bringup — SLAM 建图模式
# 启动 Gazebo + 机器人 + slam_toolbox 在线异步建图 + RViz
# =============================================================================
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
def generate_launch_description():
# ============================ 包路径 ============================
bringup_dir = get_package_share_directory('gc_bringup')
origincar_dir = get_package_share_directory('origincar_description')
# ============================ 启动参数 ============================
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
use_gazebo = LaunchConfiguration('use_gazebo', default='true')
use_rviz = LaunchConfiguration('use_rviz', default='false')
world = LaunchConfiguration('world', default=os.path.join(
origincar_dir, 'world', 'gc_world.world'))
declare_use_sim_time = DeclareLaunchArgument(
'use_sim_time', default_value='true',
description='Use simulation (Gazebo) clock if true')
declare_use_gazebo = DeclareLaunchArgument(
'use_gazebo', default_value='true',
description='Start Gazebo simulation')
declare_use_rviz = DeclareLaunchArgument(
'use_rviz', default_value='false',
description='Start RViz2 (set true if you need local visualization)')
declare_world = DeclareLaunchArgument(
'world', default_value=os.path.join(origincar_dir, 'world', 'gc_world.world'),
description='Full path to Gazebo world file')
# ============================ 机器人模型 ============================
default_model_path = os.path.join(origincar_dir, 'urdf', 'origincar.urdf')
robot_description = ParameterValue(
Command(['xacro ', default_model_path]), value_type=str)
# ============================ Gazebo ============================
start_gazebo = ExecuteProcess(
condition=IfCondition(use_gazebo),
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
'-s', 'libgazebo_ros_factory.so', world],
output='screen'
)
spawn_robot = Node(
condition=IfCondition(use_gazebo),
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', 'mycar',
'-file', default_model_path,
'-x', '0.0', '-y', '0.0', '-z', '0.1'],
output='screen'
)
# ============================ TF & 机器人状态 ============================
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[{
'robot_description': robot_description,
'use_sim_time': True,
'publish_frequency': 30.0
}],
output='screen'
)
joint_state_publisher = Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
parameters=[{'use_sim_time': True}],
output='screen'
)
# ============================ SLAM Toolbox (在线异步建图) ============================
slam_params_file = os.path.join(
bringup_dir, 'config', 'mapper_params_online_async.yaml')
slam_toolbox_node = Node(
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen',
parameters=[slam_params_file, {'use_sim_time': True}],
remappings=[('/scan', '/scan')]
)
# ============================ RViz2 ============================
rviz_config = os.path.join(bringup_dir, 'rviz', 'slam_mapping.rviz')
rviz_node = Node(
condition=IfCondition(use_rviz),
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config],
parameters=[{'use_sim_time': True}],
output='screen'
)
# ============================ 键盘遥控 (用于驱动机器人建图) ============================
use_teleop = LaunchConfiguration('use_teleop', default='true')
declare_use_teleop = DeclareLaunchArgument(
'use_teleop', default_value='true',
description='Start keyboard teleop for driving')
teleop_node = Node(
condition=IfCondition(use_teleop),
package='teleop_twist_keyboard',
executable='teleop_twist_keyboard',
name='teleop_twist_keyboard',
prefix='xterm -e',
output='screen',
remappings=[('/cmd_vel', '/cmd_vel')],
parameters=[{'use_sim_time': True}]
)
# ============================ 组装启动描述 ============================
ld = LaunchDescription()
ld.add_action(declare_use_sim_time)
ld.add_action(declare_use_gazebo)
ld.add_action(declare_use_rviz)
ld.add_action(declare_world)
ld.add_action(declare_use_teleop)
ld.add_action(start_gazebo)
# 等 Gazebo 启动后再生成机器人 (延迟 5 秒)
ld.add_action(TimerAction(
period=5.0,
actions=[spawn_robot]
))
ld.add_action(robot_state_publisher)
ld.add_action(joint_state_publisher)
# 等机器人生成后再启动 SLAM (延迟 3 秒)
ld.add_action(TimerAction(
period=8.0,
actions=[slam_toolbox_node, teleop_node, rviz_node]
))
return ld

BIN
src/gc_bringup/maps/my_map.pgm Normal file
View File

Binary file not shown.

BIN
src/gc_bringup/maps/my_map.posegraph Normal file
View File

Binary file not shown.

7
src/gc_bringup/maps/my_map.yaml Normal file
View File

@@ -0,0 +1,7 @@
image: my_map.pgm
mode: trinary
resolution: 0.05
origin: [-9.95, -8.29, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

28
src/gc_bringup/package.xml Normal file
View File

@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>gc_bringup</name>
<version>1.0.0</version>
<description>GC Robot Bringup: SLAM mapping and AMCL navigation for Ackermann robot with LiDAR</description>
<maintainer email="guoch@todo.todo">guoch</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>rviz2</exec_depend>
<exec_depend>nav2_bringup</exec_depend>
<exec_depend>slam_toolbox</exec_depend>
<exec_depend>origincar_description</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>teleop_twist_keyboard</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

133
src/gc_bringup/rviz/navigation.rviz Normal file
View File

@@ -0,0 +1,133 @@
Panels:
- Class: rviz_common/Displays
Name: Displays
- Class: rviz_common/Views
Name: Views
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.8
Class: rviz_default_plugins/RobotModel
Enabled: true
Name: RobotModel
Robot Description: robot_description
Visual Enabled: true
- Class: rviz_default_plugins/TF
Enabled: true
Name: TF
Frame Timeout: 15
Marker Scale: 0.3
- Class: rviz_default_plugins/Map
Enabled: true
Name: Map
Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map
Update Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map_updates
Color Scheme: map
Draw Behind: true
Use Timestamp: false
- Class: rviz_default_plugins/LaserScan
Enabled: true
Name: LaserScan
Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /scan
Size (m): 0.02
Style: Flat Squares
Decay Time: 0.3
Color Transformer: Intensity
- Class: rviz_default_plugins/Path
Enabled: true
Name: Global Plan
Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Reliable
Value: /plan
Color: 25; 255; 0
Line Style: Dashed
- Class: rviz_default_plugins/Path
Enabled: true
Name: Local Plan
Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_plan
Color: 255; 153; 0
- Class: rviz_default_plugins/Path
Enabled: true
Name: Smoothed Plan
Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Reliable
Value: /smoothed_plan
Color: 255; 255; 0
- Class: rviz_default_plugins/Costmap
Enabled: true
Name: Local Costmap
Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/costmap
Color Scheme: costmap
Draw Behind: false
- Class: rviz_default_plugins/Costmap
Enabled: false
Name: Global Costmap
Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/costmap
Color Scheme: costmap
Draw Behind: false
- Alpha: 0.99
Class: rviz_default_plugins/ParticleCloud
Enabled: true
Name: AMCL Particle Swarm
Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Reliable
Value: /particle_cloud
Color: 255; 0; 0
Style: Flat Squares
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/SetInitialPose
Topic:
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Value: /goal_pose
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 5.0
Focal Point:
X: 0.0
Y: 0.0
Z: 0.5
Name: Current View
Pitch: 0.8
Yaw: 2.8

84
src/gc_bringup/rviz/slam_mapping.rviz Normal file
View File

@@ -0,0 +1,84 @@
Panels:
- Class: rviz_common/Displays
Name: Displays
- Class: rviz_common/Views
Name: Views
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.8
Class: rviz_default_plugins/RobotModel
Enabled: true
Name: RobotModel
Robot Description: robot_description
Visual Enabled: true
- Class: rviz_default_plugins/TF
Enabled: true
Name: TF
Frame Timeout: 15
Marker Scale: 0.3
- Class: rviz_default_plugins/Map
Enabled: true
Name: Map
Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map
Update Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map_updates
Color Scheme: map
Draw Behind: true
Use Timestamp: false
- Class: rviz_default_plugins/LaserScan
Enabled: true
Name: LaserScan
Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /scan
Size (m): 0.02
Style: Flat Squares
Decay Time: 0.3
Color Transformer: Intensity
- Class: rviz_default_plugins/Path
Enabled: true
Name: Path (SLAM)
Topic:
Depth: 1
History Policy: Keep Last
Reliability Policy: Reliable
Value: /plan
Color: 25; 255; 0
Line Style: Dashed
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/SetInitialPose
Topic:
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Value: /goal_pose
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 5.0
Focal Point:
X: 0.0
Y: 0.0
Z: 0.5
Name: Current View
Pitch: 0.8
Yaw: 2.8

58
src/gc_bringup/scripts/nav_goal.py Normal file
View File

@@ -0,0 +1,58 @@
#!/usr/bin/env python3
"""
命令行发送导航目标点到 Nav2
用法:
ros2 run gc_bringup nav_goal 1.0 2.0 0.5 # 去 (1.0, 2.0), yaw=0.5
ros2 run gc_bringup nav_goal 1.0 2.0 # 去 (1.0, 2.0), yaw=0.0
ros2 run gc_bringup nav_goal -2.0 0.0 3.14 # 去 (-2.0, 0.0), 面朝后
# 或使用 topic pub
ros2 topic pub /goal_pose geometry_msgs/PoseStamped "{header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 0.0}, orientation: {z: 0.0, w: 1.0}}}"
"""
import sys
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
import math
class GoalSender(Node):
def __init__(self):
super().__init__('goal_sender')
self.publisher = self.create_publisher(PoseStamped, '/goal_pose', 10)
def send_goal(self, x: float, y: float, yaw: float = 0.0):
msg = PoseStamped()
msg.header.frame_id = 'map'
msg.header.stamp = self.get_clock().now().to_msg()
msg.pose.position.x = x
msg.pose.position.y = y
msg.pose.position.z = 0.0
msg.pose.orientation.z = math.sin(yaw / 2.0)
msg.pose.orientation.w = math.cos(yaw / 2.0)
self.publisher.publish(msg)
self.get_logger().info(f'发送导航目标: x={x:.2f}, y={y:.2f}, yaw={yaw:.2f}')
def main():
rclpy.init()
sender = GoalSender()
args = sys.argv[1:]
if len(args) < 2:
print("用法: ros2 run gc_bringup nav_goal <x> <y> [yaw]")
print("示例: ros2 run gc_bringup nav_goal 1.0 2.0 0.5")
sys.exit(1)
x = float(args[0])
y = float(args[1])
yaw = float(args[2]) if len(args) > 2 else 0.0
sender.send_goal(x, y, yaw)
rclpy.shutdown()
if __name__ == '__main__':
main()

0
src/gc_navigation/gc_navigation/__init__.py Normal file
View File

18
src/gc_navigation/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>gc_navigation</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="guoch@todo.todo">guoch</maintainer>
<license>TODO: License declaration</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
src/gc_navigation/resource/gc_navigation Normal file
View File

4
src/gc_navigation/setup.cfg Normal file
View File

@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/gc_navigation
[install]
install_scripts=$base/lib/gc_navigation

25
src/gc_navigation/setup.py Normal file
View File

@@ -0,0 +1,25 @@
from setuptools import find_packages, setup
package_name = 'gc_navigation'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='guoch',
maintainer_email='guoch@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
],
},
)

25
src/gc_navigation/test/test_copyright.py Normal file
View File

@@ -0,0 +1,25 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# 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.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'

25
src/gc_navigation/test/test_flake8.py Normal file
View File

@@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# 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.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)

23
src/gc_navigation/test/test_pep257.py Normal file
View File

@@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# 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.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'

31
src/gc_navigation2_slamtoolbox/CMakeLists.txt Normal file
View File

@@ -0,0 +1,31 @@
cmake_minimum_required(VERSION 3.8)
project(gc_navigation2_slamtoolbox)
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)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
# 查找slam_toolbox包
find_package(slam_toolbox REQUIRED)
install(
DIRECTORY launch config params maps
DESTINATION share/${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()

89
src/gc_navigation2_slamtoolbox/config/mapper_params_lifelong.yaml Normal file
View File

@@ -0,0 +1,89 @@
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: /scan
use_map_saver: true
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
restamp_tf: false
min_laser_range: 0.0 #for rastering images
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
check_min_dist_and_heading_precisely: false
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
min_pass_through: 2
occupancy_threshold: 0.1

72
src/gc_navigation2_slamtoolbox/config/mapper_params_localization.yaml Normal file
View File

@@ -0,0 +1,72 @@
slam_toolbox:
ros__parameters:
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: /scan
mode: localization #localization
# if you'd like to start localizing on bringup in a map and pose
# map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
# map_start_pose: [0.0, 0.0, 0.0]
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
restamp_tf: false
min_laser_range: 0.0 #for rastering images
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
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
check_min_dist_and_heading_precisely: false
scan_buffer_size: 3
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
do_loop_closing: true
loop_match_minimum_chain_size: 3
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
loop_search_maximum_distance: 3.0
# 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
min_pass_through: 2
occupancy_threshold: 0.1

71
src/gc_navigation2_slamtoolbox/config/mapper_params_offline.yaml Normal file
View File

@@ -0,0 +1,71 @@
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_footprint
scan_topic: /scan
use_map_saver: true
mode: mapping #localization
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 10.0
resolution: 0.05
restamp_tf: false
min_laser_range: 0.0 #for rastering images
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 14400.
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
check_min_dist_and_heading_precisely: false
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
min_pass_through: 2
occupancy_threshold: 0.1

79
src/gc_navigation2_slamtoolbox/config/mapper_params_online_async.yaml Normal file
View File

@@ -0,0 +1,79 @@
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_footprint
scan_topic: /scan
use_map_saver: true
mode: localization #localization
# 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: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
# 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
restamp_tf: false
min_laser_range: 0.0 #for rastering images
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
check_min_dist_and_heading_precisely: false
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
min_pass_through: 2
occupancy_threshold: 0.1

74
src/gc_navigation2_slamtoolbox/config/mapper_params_online_multi_async.yaml Normal file
View File

@@ -0,0 +1,74 @@
$(var namespace)/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: global_odom
map_frame: map
base_frame: base_footprint
scan_topic: scan
mode: mapping #localization
# 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
check_min_dist_and_heading_precisely: false
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

79
src/gc_navigation2_slamtoolbox/config/mapper_params_online_sync.yaml Normal file
View File

@@ -0,0 +1,79 @@
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_footprint
scan_topic: /scan
use_map_saver: true
mode: mapping #localization
# 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
restamp_tf: false
min_laser_range: 0.0 #for rastering images
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
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
check_min_dist_and_heading_precisely: false
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
min_pass_through: 2
occupancy_threshold: 0.1

137
src/gc_navigation2_slamtoolbox/config/slam_toolbox_default.rviz Normal file
View File

@@ -0,0 +1,137 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 154
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
- Class: slam_toolbox::SlamToolboxPlugin
Name: SlamToolboxPlugin
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 10
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.785398006439209
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.785398006439209
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000217000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000125000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e0100000168000001850000018500ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000017e000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
SlamToolboxPlugin:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1200
X: 72
Y: 60

55
src/gc_navigation2_slamtoolbox/config/slam_toolbox_localization.yaml Normal file
View File

@@ -0,0 +1,55 @@
slam_toolbox:
ros__parameters:
# 核心模式设置
use_sim_time: true
mode: "localization" # 重要：设置为定位模式
map_file_name: "" # 由启动参数传入
map_start_pose: [0.0,0.0,0.0]
# ROS框架设置
odom_frame: "odom"
map_frame: "map"
base_frame: "base_footprint"
scan_topic: "/scan"
# 求解器设置
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"
# 定位优化参数
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.05
minimum_travel_heading: 0.087
# 扫描处理
scan_buffer_size: 50
throttle_scans: 2
# 回环检测（定位模式可关闭）
do_loop_closing: false
# 搜索参数
link_match_minimum_response_fine: 0.2
link_scan_maximum_distance: 2.0
loop_search_maximum_distance: 5.0
# 性能参数
minimum_time_interval: 0.1
transform_publish_period: 0.02
map_update_interval: 0.5
# 地图参数
resolution: 0.05
max_laser_range: 15.0
min_laser_range: 0.2
# 初始位姿（可选，可以通过RViz设置）
# map_start_pose: [0.0, 0.0, 0.0]
# 调试
debug_logging: false
enable_interactive_mode: true

170
src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slamtoolbox.launch.py Normal file
View File

@@ -0,0 +1,170 @@
from launch.actions import IncludeLaunchDescription
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
# def generate_launch_description():
# # 获取包路径
# gc_navigation2_dir = get_package_share_directory(
# 'gc_navigation2_slamtoolbox')
# nav2_bringup_dir = get_package_share_directory('nav2_bringup')
# # 声明参数
# use_sim_time = LaunchConfiguration('use_sim_time', default='True')
# slam = LaunchConfiguration('slam', default='True')
# localization = LaunchConfiguration('localization', default='True')
# # 定义文件路径
# map_yaml_path = os.path.join(gc_navigation2_dir, 'maps', 'my_map.yaml')
# nav2_param_path = os.path.join(
# gc_navigation2_dir, 'params', 'gc_navigation.yaml')
# slam_params_file = os.path.join(
# gc_navigation2_dir, 'config', 'slam_toolbox_localization.yaml')
# # 定义地图基础路径（用于 .posegraph 文件）
# map_base_path = os.path.join(gc_navigation2_dir, 'maps', 'my_map')
# # 检查 .posegraph 文件是否存在
# posegraph_path = map_base_path + '.posegraph'
# if not os.path.exists(posegraph_path):
# print(f"警告: .posegraph 文件不存在: {posegraph_path}")
# print("将使用 AMCL 替代")
# use_slam_toolbox = False
# else:
# use_slam_toolbox = True
# print(f"找到 .posegraph 文件: {posegraph_path}")
# # 创建启动描述
# ld = LaunchDescription([
# DeclareLaunchArgument('use_sim_time', default_value='True'),
# DeclareLaunchArgument('slam', default_value='True'),
# DeclareLaunchArgument('localization', default_value='True'),
# ])
# # 如果使用 slam_toolbox
# if use_slam_toolbox:
# slam_toolbox_node = Node(
# package='slam_toolbox',
# executable='localization_slam_toolbox_node',
# name='slam_toolbox',
# output='screen',
# parameters=[
# slam_params_file,
# {'map_file_name': map_base_path}
# ]
# )
# ld.add_action(slam_toolbox_node)
# # 使用 slam_toolbox 时，Nav2 不启用定位
# localization_arg = 'False'
# print("slam成功配置")
# ld.add_action(TimerAction(
# period=1.5,
# actions=[]
# ))
# else:
# # 使用 AMCL
# localization_arg = 'True'
# # Nav2 启动 - 使用正确的路径
# nav2_launch_path = os.path.join(
# nav2_bringup_dir, 'launch', 'bringup_launch.py')
# if not os.path.exists(nav2_launch_path):
# print(f"错误: 找不到 Nav2 启动文件: {nav2_launch_path}")
# return ld
# else:
# print("nav2桥，通过")
# nav2_bringup_launch = IncludeLaunchDescription(
# PythonLaunchDescriptionSource(nav2_launch_path),
# launch_arguments={
# 'map': map_yaml_path,
# 'use_sim_time': use_sim_time,
# 'params_file': nav2_param_path,
# 'slam': slam,
# 'localization': localization_arg,
# 'autostart': 'True',
# }.items(),
# )
# print("结束配置")
# ld.add_action(nav2_bringup_launch)
# return ld
# def generate_launch_description():
# ld = LaunchDescription()
# # 获取参数文件路径
# slam_pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
# slam_file = os.path.join(
# slam_pkg_share, 'config', 'slam_toolbox_localization.yaml')
# use_sim_time = LaunchConfiguration('use_sim_time', default='true')
# map_base_path = os.path.join(slam_pkg_share, 'maps', 'my_map')
# posegraph_path = map_base_path+'.posegraph'
# # 创建SLAM Toolbox节点
# slam_toolbox_node = Node(
# package='slam_toolbox',
# executable='async_slam_toolbox_node', # 或 sync_slam_toolbox_node
# name='slam_toolbox',
# output='screen',
# parameters=[slam_file,
# {
# 'map_file_name': map_base_path,
# 'use_sim_time': use_sim_time
# }]
# )
# # ld.add_action(slam_toolbox_node)
# nav2_bringup_dir = get_package_share_directory('nav2_bringup')
# nav_pkg = get_package_share_directory(
# 'gc_navigation2_slamtoolbox')
# map_yaml_path = LaunchConfiguration('map', default=os.path.join(
# nav_pkg, 'maps', 'my_map.yaml'))
# nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
# nav_pkg, 'params', 'gc_navigation.yaml'))
# nav2_bringup_launch = IncludeLaunchDescription(
# PythonLaunchDescriptionSource(
# [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
# launch_arguments={
# 'map': map_yaml_path,
# 'use_sim_time': use_sim_time,
# 'params_file': nav2_param_path}.items(),
# )
# time=TimerAction(period=2.0,
# actions=[])
# ld.add_action(time)
# ld.add_action(nav2_bringup_launch)
# return ld
def generate_launch_description():
# =============================1.定位到包的地址=============================================================
gc_navigation_fish_dir = get_package_share_directory(
'gc_navigation2_slamtoolbox')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
# =============================2.声明参数，获取配置文件路径===================================================
# use_sim_time 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
gc_navigation_fish_dir, 'maps', 'my_map.yaml'))
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
gc_navigation_fish_dir, 'params', 'gc_navigation.yaml'))
# =============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
nav2_bringup_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[nav2_bringup_dir, '/launch', '/bringup_launch.py']),
launch_arguments={
'map': map_yaml_path,
'use_sim_time': use_sim_time,
'params_file': nav2_param_path}.items(),
)
return LaunchDescription([nav2_bringup_launch])

25
src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_lifelong.launch.py Normal file
View File

@@ -0,0 +1,25 @@
#!/usr/bin/env python3
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
# 获取参数文件路径
pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
params_file = os.path.join(
pkg_share, 'config', 'mapper_params_lifelong.yaml')
# 创建SLAM Toolbox节点
slam_toolbox_node = Node(
package='slam_toolbox',
executable='async_slam_toolbox_node', # 或 sync_slam_toolbox_node
name='slam_toolbox',
output='screen',
parameters=[params_file]
)
return LaunchDescription([
slam_toolbox_node
])

25
src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_location.launch.py Normal file
View File

@@ -0,0 +1,25 @@
#!/usr/bin/env python3
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
# 获取参数文件路径
pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
params_file = os.path.join(
pkg_share, 'config', 'mapper_params_localization.yaml')
# 创建SLAM Toolbox节点
slam_toolbox_node = Node(
package='slam_toolbox',
executable='localization_slam_toolbox_node', # 或 sync_slam_toolbox_node
name='slam_toolbox',
output='screen',
parameters=[params_file]
)
return LaunchDescription([
slam_toolbox_node
])

25
src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_on_async.launch.py Normal file
View File

@@ -0,0 +1,25 @@
#!/usr/bin/env python3
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
# 获取参数文件路径
pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
params_file = os.path.join(
pkg_share, 'config', 'mapper_params_online_async.yaml')
# 创建SLAM Toolbox节点
slam_toolbox_node = Node(
package='slam_toolbox',
executable='async_slam_toolbox_node', # 或 sync_slam_toolbox_node
name='slam_toolbox',
output='screen',
parameters=[params_file]
)
return LaunchDescription([
slam_toolbox_node
])

BIN
src/gc_navigation2_slamtoolbox/maps/my_map.data Normal file
View File

Binary file not shown.

BIN
src/gc_navigation2_slamtoolbox/maps/my_map.pgm Normal file
View File

Binary file not shown.

BIN
src/gc_navigation2_slamtoolbox/maps/my_map.posegraph Normal file
View File

Binary file not shown.

7
src/gc_navigation2_slamtoolbox/maps/my_map.yaml Normal file
View File

@@ -0,0 +1,7 @@
image: map_1770592084.pgm
mode: trinary
resolution: 0.05
origin: [-2.15, -5.81, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

18
src/gc_navigation2_slamtoolbox/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>gc_navigation2_slamtoolbox</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="guoch@todo.todo">guoch</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

413
src/gc_navigation2_slamtoolbox/params/gc_navigation.yaml Normal file
View File

@@ -0,0 +1,413 @@
amcl:
ros__parameters:
use_sim_time: True
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 0.2
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: 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: 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.2
tf_broadcast: true
transform_tolerance: 10.0
update_min_a: 0.2
update_min_d: 0.25
z_hit: 0.5
z_max: 0.05
z_rand: 0.5
z_short: 0.05
scan_topic: scan
amcl_map_client:
ros__parameters:
use_sim_time: True
amcl_rclcpp_node:
ros__parameters:
use_sim_time: True
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
bt_loop_duration: 20
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_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
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 30.0
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 56
model_dt: 0.05
batch_size: 2000
vx_std: 0.2
vy_std: 0.0
wz_std: 0.4
vx_max: 0.5
vx_min: -0.35
vy_max: 0.0
wz_max: 1.9
iteration_count: 1
temperature: 0.3
gamma: 0.015
motion_model: "Ackermann"
visualize: false
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstraints:
min_turning_r: 0.4
critics: ["ConstraintCritic", "CostCritic", "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
# Option to replace Cost and use Obstacles instead
# ObstaclesCritic:
# enabled: true
# cost_power: 1
# repulsion_weight: 1.5
# critical_weight: 20.0
# consider_footprint: false
# collision_cost: 10000.0
# collision_margin_distance: 0.1
# near_goal_distance: 0.5
CostCritic:
enabled: true
cost_power: 1
cost_weight: 3.81
critical_cost: 300.0
consider_footprint: true
collision_cost: 1000000.0
near_goal_distance: 1.0
trajectory_point_step: 2
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 4
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
forward_preference: true
# VelocityDeadbandCritic:
# enabled: true
# cost_power: 1
# cost_weight: 35.0
# deadband_velocities: [0.05, 0.05, 0.05]
# TwirlingCritic:
# enabled: true
# twirling_cost_power: 1
# twirling_cost_weight: 10.0
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
transform_tolerance: 0.5
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 3
height: 3
resolution: 0.05
footprint: "[[0.14, 0.085],
[0.14, -0.085],
[-0.14, -0.085],
[-0.14, 0.085]]"
footprint_padding: 0.02
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
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
static_layer:
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:
use_sim_time: True
transform_tolerance: 0.5
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
footprint: "[[0.14, 0.085],
[0.14, -0.085],
[-0.14, -0.085],
[-0.14, 0.085]]"
footprint_padding: 0.02
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /scan
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
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: "turtlebot3_world.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:
planner_plugins: ["GridBased"]
use_sim_time: True
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" # or REEDS_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: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
minimum_turning_radius: 0.40 # 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: 2.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.
viz_expansions: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance.
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
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: 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", "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.5
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:
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

29
src/gc_navigation_fish/CMakeLists.txt Normal file
View File

@@ -0,0 +1,29 @@
cmake_minimum_required(VERSION 3.8)
project(gc_navigation_fish)
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)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
install(
DIRECTORY launch param maps
DESTINATION share/${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()

34
src/gc_navigation_fish/launch/gc_navigation.launch.py Normal file
View File

@@ -0,0 +1,34 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
# =============================1.定位到包的地址=============================================================
gc_navigation_fish_dir = get_package_share_directory(
'gc_navigation_fish')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
# =============================2.声明参数，获取配置文件路径===================================================
# use_sim_time 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
gc_navigation_fish_dir, 'maps', 'my_map.yaml'))
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
gc_navigation_fish_dir, 'param', 'gc_navigation.yaml'))
# =============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
nav2_bringup_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[nav2_bringup_dir, '/launch', '/bringup_launch.py']),
launch_arguments={
'map': map_yaml_path,
'use_sim_time': use_sim_time,
'params_file': nav2_param_path}.items(),
)
return LaunchDescription([nav2_bringup_launch])

BIN
src/gc_navigation_fish/maps/my_map.data Normal file
View File

Binary file not shown.

BIN
src/gc_navigation_fish/maps/my_map.pgm Normal file
View File

Binary file not shown.

BIN
src/gc_navigation_fish/maps/my_map.posegraph Normal file
View File

Binary file not shown.

7
src/gc_navigation_fish/maps/my_map.yaml Normal file
View File

@@ -0,0 +1,7 @@
image: map_1770592084.pgm
mode: trinary
resolution: 0.05
origin: [-2.15, -5.81, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

BIN
src/gc_navigation_fish/maps/test_map.pgm Normal file
View File

Binary file not shown.

7
src/gc_navigation_fish/maps/test_map.yaml Normal file
View File

@@ -0,0 +1,7 @@
image: test_map.pgm
mode: trinary
resolution: 0.05
origin: [-2.03, -6.12, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

18
src/gc_navigation_fish/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>gc_navigation_fish</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="guoch@todo.todo">guoch</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<exec_depend>nav2_bringup</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

408
src/gc_navigation_fish/param/gc_navigation.yaml Normal file
View File

@@ -0,0 +1,408 @@
# amcl:
# ros__parameters:
# use_sim_time: True
# alpha1: 0.2
# alpha2: 0.2
# alpha3: 0.2
# alpha4: 0.2
# alpha5: 0.2
# 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: 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: 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.2
# tf_broadcast: true
# transform_tolerance: 10.0
# update_min_a: 0.2
# update_min_d: 0.25
# z_hit: 0.5
# z_max: 0.05
# z_rand: 0.5
# z_short: 0.05
# scan_topic: scan
# amcl_map_client:
# ros__parameters:
# use_sim_time: True
# amcl_rclcpp_node:
# ros__parameters:
# use_sim_time: True
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
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_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
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
controller_frequency: 30.0
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 56
model_dt: 0.05
batch_size: 2000
vx_std: 0.2
vy_std: 0.0
wz_std: 0.4
vx_max: 0.5
vx_min: -0.35
vy_max: 0.0
wz_max: 1.9
iteration_count: 1
temperature: 0.3
gamma: 0.015
motion_model: "Ackermann"
visualize: false
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstraints:
min_turning_r: 0.4
critics: ["ConstraintCritic", "CostCritic", "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
# Option to replace Cost and use Obstacles instead
# ObstaclesCritic:
# enabled: true
# cost_power: 1
# repulsion_weight: 1.5
# critical_weight: 20.0
# consider_footprint: false
# collision_cost: 10000.0
# collision_margin_distance: 0.1
# near_goal_distance: 0.5
CostCritic:
enabled: true
cost_power: 1
cost_weight: 3.81
critical_cost: 300.0
consider_footprint: true
collision_cost: 1000000.0
near_goal_distance: 1.0
trajectory_point_step: 2
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 4
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
forward_preference: true
# VelocityDeadbandCritic:
# enabled: true
# cost_power: 1
# cost_weight: 35.0
# deadband_velocities: [0.05, 0.05, 0.05]
# TwirlingCritic:
# enabled: true
# twirling_cost_power: 1
# twirling_cost_weight: 10.0
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 3
height: 3
resolution: 0.05
footprint: "[[0.14, 0.085],
[0.14, -0.085],
[-0.14, -0.085],
[-0.14, 0.085]]"
footprint_padding: 0.02
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
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
static_layer:
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
footprint: "[[0.14, 0.085],
[0.14, -0.085],
[-0.14, -0.085],
[-0.14, 0.085]]"
footprint_padding: 0.02
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /scan
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
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: "turtlebot3_world.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:
planner_plugins: ["GridBased"]
use_sim_time: True
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" # or REEDS_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: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
minimum_turning_radius: 0.40 # 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: 2.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.
viz_expansions: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance.
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
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: 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", "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: 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:
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

29
src/gc_slam_toolbox_fish/CMakeLists.txt Normal file
View File

@@ -0,0 +1,29 @@
cmake_minimum_required(VERSION 3.8)
project(gc_slam_toolbox_fish)
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)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
install(
DIRECTORY config launch rviz
DESTINATION share/${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()

63
src/gc_slam_toolbox_fish/config/gc_2d.lua Normal file
View File

@@ -0,0 +1,63 @@
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
-- base_link改为odom,发布map到odom之间的位姿态
published_frame = "odom",
odom_frame = "odom",
-- true改为false，不用提供里程计数据
provide_odom_frame = false,
-- false改为true，仅发布2D位资
publish_frame_projected_to_2d = true,
-- false改为true，使用里程计数据
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
-- 0改为1,使用一个雷达
num_laser_scans = 1,
-- 1改为0，不使用多波雷达
num_multi_echo_laser_scans = 0,
-- 10改为1，1/1=1等于不分割
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
-- false改为true，启动2D SLAM
MAP_BUILDER.use_trajectory_builder_2d = true
-- 0改成0.10,比机器人半径小的都忽略
TRAJECTORY_BUILDER_2D.min_range = 0.10
-- 30改成3.5,限制在雷达最大扫描范围内，越小一般越精确些
TRAJECTORY_BUILDER_2D.max_range = 3.5
-- 5改成3,传感器数据超出有效范围最大值
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.
-- true改成false,不使用IMU数据，大家可以开启，然后对比下效果
TRAJECTORY_BUILDER_2D.use_imu_data = false
-- false改成true,使用实时回环检测来进行前端的扫描匹配
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
-- 1.0改成0.1,提高对运动的敏感度
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
-- 0.55改成0.65,Fast csm的最低分数，高于此分数才进行优化。
POSE_GRAPH.constraint_builder.min_score = 0.65
--0.6改成0.7,全局定位最小分数，低于此分数则认为目前全局定位不准确
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
-- 设置0可关闭全局SLAM
-- POSE_GRAPH.optimize_every_n_nodes = 0
return options

63
src/gc_slam_toolbox_fish/launch/catograph.launch.py Normal file
View File

@@ -0,0 +1,63 @@
import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# 定位到功能包的地址
pkg_share = FindPackageShare(
package='gc_slam_toolbox_fish').find('gc_slam_toolbox_fish')
# =====================运行节点需要的配置=======================================================================
# 是否使用仿真时间，我们用gazebo，这里设置成true
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
# 地图的分辨率
resolution = LaunchConfiguration('resolution', default='0.05')
# 地图的发布周期
publish_period_sec = LaunchConfiguration(
'publish_period_sec', default='1.0')
# 配置文件夹路径
configuration_directory = LaunchConfiguration(
'configuration_directory', default=os.path.join(pkg_share, 'config'))
# 配置文件
configuration_basename = LaunchConfiguration(
'configuration_basename', default='gc_2d.lua')
rviz_config_dir = os.path.join(pkg_share, 'config')+"/cartographer.rviz"
print(f"rviz config in {rviz_config_dir}")
# =====================声明三个节点，cartographer/occupancy_grid_node/rviz_node=================================
cartographer_node = Node(
package='cartographer_ros',
executable='cartographer_node',
name='cartographer_node',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
arguments=['-configuration_directory', configuration_directory,
'-configuration_basename', configuration_basename])
cartographer_occupancy_grid_node = Node(
package='cartographer_ros',
executable='cartographer_occupancy_grid_node',
name='cartographer_occupancy_grid_node',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec])
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_dir],
parameters=[{'use_sim_time': use_sim_time}],
output='screen')
# ===============================================定义启动文件========================================================
ld = LaunchDescription()
ld.add_action(cartographer_node)
ld.add_action(cartographer_occupancy_grid_node)
# ld.add_action(rviz_node)
return ld

18
src/gc_slam_toolbox_fish/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>gc_slam_toolbox_fish</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="guoch@todo.todo">guoch</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

63
src/gc_slamtoolbox/config/nav2_costmap_2d.lua Normal file
View File

@@ -0,0 +1,63 @@
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
-- base_link改为odom,发布map到odom之间的位姿态
published_frame = "odom",
odom_frame = "odom",
-- true改为false，不用提供里程计数据
provide_odom_frame = false,
-- false改为true，仅发布2D位资
publish_frame_projected_to_2d = true,
-- false改为true，使用里程计数据
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
-- 0改为1,使用一个雷达
num_laser_scans = 1,
-- 1改为0，不使用多波雷达
num_multi_echo_laser_scans = 0,
-- 10改为1，1/1=1等于不分割
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
-- false改为true，启动2D SLAM
MAP_BUILDER.use_trajectory_builder_2d = true
-- 0改成0.10,比机器人半径小的都忽略
TRAJECTORY_BUILDER_2D.min_range = 0.10
-- 30改成3.5,限制在雷达最大扫描范围内，越小一般越精确些
TRAJECTORY_BUILDER_2D.max_range = 3.5
-- 5改成3,传感器数据超出有效范围最大值
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.
-- true改成false,不使用IMU数据，大家可以开启，然后对比下效果
TRAJECTORY_BUILDER_2D.use_imu_data = false
-- false改成true,使用实时回环检测来进行前端的扫描匹配
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
-- 1.0改成0.1,提高对运动的敏感度
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
-- 0.55改成0.65,Fast csm的最低分数，高于此分数才进行优化。
POSE_GRAPH.constraint_builder.min_score = 0.65
--0.6改成0.7,全局定位最小分数，低于此分数则认为目前全局定位不准确
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
-- 设置0可关闭全局SLAM
-- POSE_GRAPH.optimize_every_n_nodes = 0
return options

0
src/gc_slamtoolbox/gc_slamtoolbox/__init__.py Normal file
View File

63
src/gc_slamtoolbox/launch/cartographer.launch.py Normal file
View File

@@ -0,0 +1,63 @@
import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# 定位到功能包的地址
pkg_share = FindPackageShare(
package='gc_slamtoolbox').find('gc_slamtoolbox')
# =====================运行节点需要的配置=======================================================================
# 是否使用仿真时间，我们用gazebo，这里设置成true
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
# 地图的分辨率
resolution = LaunchConfiguration('resolution', default='0.05')
# 地图的发布周期
publish_period_sec = LaunchConfiguration(
'publish_period_sec', default='1.0')
# 配置文件夹路径
configuration_directory = LaunchConfiguration(
'configuration_directory', default=os.path.join(pkg_share, 'config'))
# 配置文件
configuration_basename = LaunchConfiguration(
'configuration_basename', default='nav2_costmap_2d.lua')
rviz_config_dir = os.path.join(pkg_share, 'config')+"/cartographer.rviz"
print(f"rviz config in {rviz_config_dir}")
# =====================声明三个节点，cartographer/occupancy_grid_node/rviz_node=================================
cartographer_node = Node(
package='cartographer_ros',
executable='cartographer_node',
name='cartographer_node',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
arguments=['-configuration_directory', configuration_directory,
'-configuration_basename', configuration_basename])
cartographer_occupancy_grid_node = Node(
package='cartographer_ros',
executable='cartographer_occupancy_grid_node',
name='cartographer_occupancy_grid_node',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec])
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_dir],
parameters=[{'use_sim_time': use_sim_time}],
output='screen')
# ===============================================定义启动文件========================================================
ld = LaunchDescription()
ld.add_action(cartographer_node)
ld.add_action(cartographer_occupancy_grid_node)
ld.add_action(rviz_node)
return ld

18
src/gc_slamtoolbox/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>gc_slamtoolbox</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="guoch@todo.todo">guoch</maintainer>
<license>TODO: License declaration</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
src/gc_slamtoolbox/resource/gc_slamtoolbox Normal file
View File

4
src/gc_slamtoolbox/setup.cfg Normal file
View File

@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/gc_slamtoolbox
[install]
install_scripts=$base/lib/gc_slamtoolbox

25
src/gc_slamtoolbox/setup.py Normal file
View File

@@ -0,0 +1,25 @@
from setuptools import find_packages, setup
package_name = 'gc_slamtoolbox'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='guoch',
maintainer_email='guoch@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
],
},
)

25
src/gc_slamtoolbox/test/test_copyright.py Normal file
View File

@@ -0,0 +1,25 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# 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.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'

25
src/gc_slamtoolbox/test/test_flake8.py Normal file
View File

@@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# 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.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)

23
src/gc_slamtoolbox/test/test_pep257.py Normal file
View File

@@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# 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.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'

40
src/origincar_description/CMakeLists.txt Normal file
View File

@@ -0,0 +1,40 @@
cmake_minimum_required(VERSION 3.5)
project(origincar_description)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
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)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
install(
DIRECTORY launch urdf rviz meshes world
DESTINATION share/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

1
src/origincar_description/config/joint_names_origincar_description.yaml Normal file
View File

@@ -0,0 +1 @@
controller_joint_names: ['', 'down_left_joint', 'down_right_joint', 'up_left_joint', ]

BIN
src/origincar_description/config/joint_names_origincar_description.yaml:Zone.Identifier Normal file
View File

Binary file not shown.

20
src/origincar_description/launch/display.launch Normal file
View File

@@ -0,0 +1,20 @@
<launch>
<arg
name="model" />
<param
name="robot_description"
textfile="$(find origincar_description)/urdf/origincar.urdf" />
<node
name="joint_state_publisher_gui"
pkg="joint_state_publisher_gui"
type="joint_state_publisher_gui" />
<node
name="robot_state_publisher"
pkg="robot_state_publisher"
type="robot_state_publisher" />
<node
name="rviz"
pkg="rviz"
type="rviz"
args="-d $(find origincar_description)/urdf.rviz" />
</launch>

63
src/origincar_description/launch/display.launch.py Normal file
View File

@@ -0,0 +1,63 @@
#!/usr/bin/python3
from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command, LaunchConfiguration
from launch.actions import DeclareLaunchArgument
def generate_launch_description():
# * 这里是机器人模型路径之类的
origincar_urdf_dir = get_package_share_directory('origincar_description')
default_model_path = os.path.join(
origincar_urdf_dir, "urdf", "origincar.urdf")
default_rviz_path = os.path.join(
origincar_urdf_dir, "rviz", "display.rviz")
model = DeclareLaunchArgument(
name="model", default_value=default_model_path)
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
# 加载机器人模型
# 1.启动 robot_state_publisher 节点并以参数方式加载 urdf 文件
robot_description = ParameterValue(Command(["xacro ", LaunchConfiguration("model")]),
value_type=str)
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[{"robot_description": robot_description,
'use_sim_time': True, 'publish_frequency': 30.0}]
)
joint_state_publisher_node = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui',
parameters=[{'use_sim_time': use_sim_time}],
arguments=[default_model_path]
)
# 2.启动 joint_state_publisher 节点发布非固定关节状态
joint_state_publisher = Node(
package="joint_state_publisher",
executable="joint_state_publisher"
)
# rviz2 节点
# *-d 为加载配置文件，这个不用看，因人而异
rviz2 = Node(
package="rviz2",
executable="rviz2",
parameters=[{'use_sim_time': use_sim_time}],
arguments=["-d", default_rviz_path]
)
return LaunchDescription([
model,
robot_state_publisher,
# joint_state_publisher_node,
joint_state_publisher,
rviz2
])

20
src/origincar_description/launch/gazebo.launch Normal file
View File

@@ -0,0 +1,20 @@
<launch>
<include
file="$(find gazebo_ros)/launch/empty_world.launch" />
<node
name="tf_footprint_base"
pkg="tf"
type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" />
<node
name="spawn_model"
pkg="gazebo_ros"
type="spawn_model"
args="-file $(find origincar_description)/urdf/origincar_description.urdf -urdf -model origincar_description"
output="screen" />
<node
name="fake_joint_calibration"
pkg="rostopic"
type="rostopic"
args="pub /calibrated std_msgs/Bool true" />
</launch>

68
src/origincar_description/launch/gazebo.launch.py Normal file
View File

@@ -0,0 +1,68 @@
from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command, LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch.actions import ExecuteProcess
def generate_launch_description():
ld = LaunchDescription()
robot_name_in_model = 'mycar'
origincar_urdf_dir = get_package_share_directory(
'origincar_description')
default_model_path = os.path.join(
origincar_urdf_dir, "urdf", "origincar.urdf")
default_rviz_path = os.path.join(
origincar_urdf_dir, "rviz", "display.rviz")
model = DeclareLaunchArgument(
name="model", default_value=default_model_path)
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
# * ###################gazebo
gazebo_world_path = os.path.join(origincar_urdf_dir, 'world/test.world')
start_gazebo_cmd = ExecuteProcess(
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
'-s', 'libgazebo_ros_factory.so', gazebo_world_path],
output='screen')
spawn_entity_cmd = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', robot_name_in_model,
'-file', default_model_path],
output='screen'
)
robot_description = ParameterValue(Command(["xacro ", LaunchConfiguration("model")]),
value_type=str)
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[{"robot_description": robot_description,
'use_sim_time': True, 'publish_frequency': 30.0}]
)
# 2.启动 joint_state_publisher 节点发布非固定关节状态
joint_state_publisher = Node(
package="joint_state_publisher",
executable="joint_state_publisher"
)
# *###################rviz
# rviz2 节点
# *-d 为加载配置文件，这个不用看，因人而异
rviz2 = Node(
package="rviz2",
executable="rviz2",
arguments=["-d", default_rviz_path],
parameters=[{'use_sim_time': use_sim_time}]
)
ld.add_action(model)
ld.add_action(start_gazebo_cmd)
ld.add_action(spawn_entity_cmd)
ld.add_action(robot_state_publisher)
# ld.add_action(joint_state_publisher)
# ld.add_action(rviz2)
return ld

BIN
src/origincar_description/meshes/base_link.STL Normal file
View File

Binary file not shown.

BIN
src/origincar_description/meshes/base_link.STL:Zone.Identifier Normal file
View File

Binary file not shown.

BIN
src/origincar_description/meshes/down_left_Link.STL Normal file
View File

Binary file not shown.

BIN
src/origincar_description/meshes/down_left_Link.STL:Zone.Identifier Normal file
View File

Binary file not shown.

BIN
src/origincar_description/meshes/down_right_Link.STL Normal file
View File

Binary file not shown.

BIN
src/origincar_description/meshes/down_right_Link.STL:Zone.Identifier Normal file
View File

Binary file not shown.

BIN
src/origincar_description/meshes/up_left_Link.STL Normal file
View File

Binary file not shown.

BIN
src/origincar_description/meshes/up_left_Link.STL:Zone.Identifier Normal file
View File

Binary file not shown.

BIN
src/origincar_description/meshes/up_right_Link.STL Normal file
View File

Binary file not shown.

BIN
src/origincar_description/meshes/up_right_Link.STL:Zone.Identifier Normal file
View File

Binary file not shown.

24
src/origincar_description/package.xml Normal file
View File

@@ -0,0 +1,24 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>origincar_description</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ps-micro@todo.todo">ps-micro</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<exec_depend>rviz2</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>ros2launch</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

1
src/origincar_description/rviz/README Normal file
View File

@@ -0,0 +1 @@
## 此文件夹为配置RVIZ，暂时不使用

186
src/origincar_description/rviz/display.rviz Normal file
View File

@@ -0,0 +1,186 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
- /RobotModel1/Description Topic1
Splitter Ratio: 0.5
Tree Height: 490
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
down_left_Link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
down_right_Link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
up_left_Link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
up_right_Link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz_default_plugins/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
{}
Update Interval: 0
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 0.4845520257949829
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.7803981304168701
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.05039529129862785
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 719
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000015600000275fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000275000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000275fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000275000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000035a0000027500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1483
X: 72
Y: 27

497
src/origincar_description/urdf/origincar.urdf Normal file
View File

@@ -0,0 +1,497 @@
<?xml version="1.0"?>
<robot name="mycar">
<link name="base_footprint">
<origin xyz="0 0 0" rpy="0 0 0" />
</link>
<link name="base_link">
<origin xyz="0 0 0" rpy="0 0 0" />
</link>
<joint name="base_joint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />
<origin xyz="0 0.0 -0.09" rpy="0 0 0" />
</joint>
<link name="chassis_link">
<inertial>
<origin xyz="0.0374140021041951 -0.000373005187591258 -0.0771282894414029" rpy="0 0 0" />
<mass value="12" />
<inertia ixx="2.0833333333333335"
ixy="0" ixz="0"
iyy="11.979166666666668" iyz="0"
izz="11.979166666666668" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.276 0.164 0.08" />
</geometry>
<material name="base_color">
<color rgba="0.4 0.039 0.039 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.07" />
<geometry>
<box size="0.276 0.164 0.08" />
</geometry>
</collision>
</link>
<joint name="chassis_joint" type="fixed">
<parent link="base_link" />
<child link="chassis_link" />
<origin xyz="0 0.0 0.1555" rpy="0 0 0" />
</joint>
<link name="borad_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="12" />
<inertia ixx="2.0833333333333335"
ixy="0" ixz="0"
iyy="11.979166666666668" iyz="0"
izz="11.979166666666668" />
</inertial>
<visual>
<geometry>
<box size="0.200 0.164 0.07" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 0" />
<material name="rgba(246,152,152,1)">
<color rgba="0.9647058823529412 0.596078431372549 0.596078431372549 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0" />
<geometry>
<box size="0.200 0.164 0.07" />
</geometry>
</collision>
</link>
<joint name="board_joint" type="fixed">
<parent link="chassis_link" />
<child link="borad_link" />
<origin rpy="0 0 0" xyz="0 0 0.075" />
</joint>
<link name="fr_left_steer_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.01" />
<inertia
ixx="9.880333333333333E-5"
ixy="0" ixz="0"
iyy="9.880333333333333E-5"
iyz="0" izz="9.8E-5" />
</inertial>
</link>
<joint name="fr_left_steer_joint" type="revolute">
<origin xyz="0.0841 0.0945 -0.03" rpy="0 0 0" />
<parent link="chassis_link" />
<child link="fr_left_steer_link" />
<axis xyz="0 0 1" />
<limit upper="0.6" lower="-0.6" effort="-1.0" velocity="-20.0" />
</joint>
<link name="fr_left_wheel_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1.5" />
<inertia
ixx="0.02964"
ixy="0" ixz="0"
iyy="0.02964"
iyz="0" izz="0.0294" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.025" radius="0.03" />
</geometry>
<material name="">
<color rgba="0.223529411764706 0.223529411764706 0.223529411764706 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.025" radius="0.03" />
</geometry>
</collision>
</link>
<joint name="fr_left_wheel_joint" type="continuous">
<origin xyz="0 0 0" rpy="1.5708 0 0" />
<parent link="fr_left_steer_link" />
<child link="fr_left_wheel_link" />
<axis xyz="0 0 -1" />
<dynamics friction="0.0" damping="0.01" />
<limit lower="-3.14159" upper="3.14159" effort="100" velocity="-200" />
</joint>
<link name="fr_right_steer_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.01" />
<inertia
ixx="9.880333333333333E-5"
ixy="0" ixz="0"
iyy="9.880333333333333E-5"
iyz="0" izz="9.8E-5" />
</inertial>
</link>
<joint name="fr_right_steer_joint" type="revolute">
<origin xyz="0.0841 -0.0945 -0.03" rpy="0 0 0" />
<parent link="chassis_link" />
<child link="fr_right_steer_link" />
<axis xyz="0 0 1" />
<limit upper="0.6" lower="-0.6" effort="-1.0" velocity="-20.0" />
</joint>
<link name="fr_right_wheel_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1.5" />
<inertia
ixx="0.02964"
ixy="0" ixz="0"
iyy="0.02964"
iyz="0" izz="0.0294" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.025" radius="0.03" />
</geometry>
<material name="">
<color rgba="0.223529411764706 0.223529411764706 0.223529411764706 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.025" radius="0.03" />
</geometry>
</collision>
</link>
<joint name="fr_right_wheel_joint" type="continuous">
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
<parent link="fr_right_steer_link" />
<child link="fr_right_wheel_link" />
<axis xyz="0 0 1" />
<dynamics friction="0.0" damping="0.01" />
<limit lower="-3.14159" upper="3.14159" effort="100" velocity="-200" />
</joint>
<link name="re_left_wheel_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1.5" />
<inertia
ixx="0.02964"
ixy="0" ixz="0"
iyy="0.02964"
iyz="0" izz="0.0294" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.025" radius="0.03" />
</geometry>
<material name="">
<color rgba="0.37647 0.37647 0.37647 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.025" radius="0.03" />
</geometry>
</collision>
</link>
<joint name="re_left_wheel_joint" type="continuous">
<origin xyz="-0.0841 0.0945 -0.03" rpy="1.5708 0 0" />
<parent link="chassis_link" />
<child link="re_left_wheel_link" />
<axis xyz="0 0 -1" />
<dynamics friction="0.0" damping="0.01" />
<limit lower="-3.14159" upper="3.14159" effort="100" velocity="-200" />
</joint>
<link name="re_right_wheel_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1.5" />
<inertia
ixx="0.02964"
ixy="0" ixz="0"
iyy="0.02964"
iyz="0" izz="0.0294" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.025" radius="0.03" />
</geometry>
<material name="">
<color rgba="0.37647 0.37647 0.37647 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.025" radius="0.03" />
</geometry>
</collision>
</link>
<joint name="re_right_wheel_joint" type="continuous">
<origin xyz="-0.0841 -0.0945 -0.03" rpy="-1.5708 0 0" />
<parent link="chassis_link" />
<child link="re_right_wheel_link" />
<axis xyz="0 0 1" />
<dynamics friction="0.0" damping="0.01" />
<limit lower="-3.14159" upper="3.14159" effort="100" velocity="-200" />
</joint>
<link name="virtual_steer_link">
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<geometry>
<box size="0.01 0.01 0.01" />
</geometry>
<material name="">
<color rgba="1.0 1.0 0.0 1" />
</material>
</visual>
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<mass value="0.001" />
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
</inertial>
</link>
<joint name="virtual_steering_wheel_joint" type="revolute">
<origin xyz="0.0841 0.0 0.0" rpy="0.0 0.0 0.0" />
<parent link="chassis_link" />
<child link="virtual_steer_link" />
<axis xyz="-1 0 0" />
<limit lower="-3" upper="3" effort="200" velocity="20" />
</joint>
<!-- **************************** -->
<!-- 激光雷达 -->
<link name="laser_link">
<visual>
<geometry>
<cylinder radius="0.025" length="0.05" />
</geometry>
<origin rpy="0 0 0" xyz="0.04 0 0" />
<material name="rgba(255,255,255,1)">
<color rgba="1 1 1 1" />
</material>
</visual>
</link>
<joint name="laser_joint" type="fixed">
<parent link="borad_link" />
<child link="laser_link" />
<origin rpy="0 0 0" xyz="0.045 0 0.065" />
</joint>
<!-- IMU -->
<link name="imu_link">
<visual>
<geometry>
<box size="0.02 0.02 0.02" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.01" />
<material name="">
<color rgba="1 1 1 1" />
</material>
</visual>
</link>
<joint name="imu_joint" type="fixed">
<parent link="borad_link" />
<child link="imu_link" />
<origin rpy="0 0 0" xyz="0 0 0.045" />
</joint>
<gazebo reference="fr_left_wheel_link">
<mu1>1.1</mu1>
<mu2>1.2</mu2>
</gazebo>
<gazebo reference="fr_right_wheel_link">
<mu1>1.1</mu1>
<mu2>1.2</mu2>
</gazebo>
<gazebo reference="re_left_wheel_link">
<mu1>1.1</mu1>
<mu2>1.2</mu2>
</gazebo>
<gazebo reference="re_right_wheel_link">
<mu1>1.1</mu1>
<mu2>1.2</mu2>
</gazebo>
<!-- ackerman -->
<gazebo>
<plugin name="gazebo_ros_ackermann_drive" filename="libgazebo_ros_ackermann_drive.so">
<ros>
<namespace>/</namespace>
<remapping>cmd_vel:=cmd_vel</remapping>
<remapping>odom:=odom</remapping>
<remapping>distance:=distance</remapping>
</ros>
<update_rate>30.0</update_rate>
<front_left_joint>fr_left_wheel_joint</front_left_joint>
<front_right_joint>fr_right_wheel_joint</front_right_joint>
<rear_left_joint>re_left_wheel_joint</rear_left_joint>
<rear_right_joint>re_right_wheel_joint</rear_right_joint>
<left_steering_joint>fr_left_steer_joint</left_steering_joint>
<right_steering_joint>fr_right_steer_joint</right_steering_joint>
<steering_wheel_joint>virtual_steering_wheel_joint</steering_wheel_joint>
<max_steer>0.583972</max_steer>
<max_steering_angle>1.570796</max_steering_angle>
<max_speed>4</max_speed>
<left_steering_pid_gain>600 0 6</left_steering_pid_gain>
<left_steering_i_range>0 0</left_steering_i_range>
<right_steering_pid_gain>600 0 6</right_steering_pid_gain>
<right_steering_i_range>0 0</right_steering_i_range>
<linear_velocity_pid_gain>800 0 0</linear_velocity_pid_gain>
<linear_velocity_i_range>0 0</linear_velocity_i_range>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_footprint</robot_base_frame>
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
</plugin>
<plugin name="gazebo_ros_joint_state_publisher"
filename="libgazebo_ros_joint_state_publisher.so">
<update_rate>100</update_rate>
<joint_name>fr_right_steer_joint</joint_name>
<joint_name>fr_right_wheel_joint</joint_name>
<joint_name>fr_left_steer_joint</joint_name>
<joint_name>fr_left_wheel_joint</joint_name>
<joint_name>re_right_wheel_joint</joint_name>
<joint_name>re_left_wheel_joint</joint_name>
<joint_name>virtual_steering_wheel_joint</joint_name>
<joint_name>base_joint</joint_name>
<joint_name>chassis_joint</joint_name>
<joint_name>board_joint</joint_name>
<joint_name>laser_joint</joint_name>
<joint_name>imu_joint</joint_name>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<ros>
<namespace>/</namespace>
<remapping>~/out:=imu</remapping>
</ros>
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
<gazebo reference="laser_link">
<sensor name="laser_sensor" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>5</update_rate>
<pose>0 0 0.075 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1.000000</resolution>
<min_angle>0.000000</min_angle>
<max_angle>6.280000</max_angle>
</horizontal>
</scan>
<range>
<min>0.120000</min>
<max>3.5</max>
<resolution>0.015000</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
<ros>
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>laser_link</frame_name>
</plugin>
</sensor>
</gazebo>
</robot>

135
src/origincar_description/urdf/origincar.xacro Normal file
View File

@@ -0,0 +1,135 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:property name="PI" value="3.1415926"/>
<xacro:property name="Track" value="0.1682"/>
<xacro:property name="WheelBase" value="0.189"/>
<xacro:property name="WheelLen" value="0.025"/>
<link name="base_link">
<visual>
<geometry>
<box size="0.276 ${WheelBase-WheelLen} 0.08"/>
</geometry>
<origin xyz="0 0 ${0.08/2+0.03}" rpy="0 0 0"/>
<material name="rgba(102,10,10,1)">
<color rgba="${102/255} ${10/255} ${10/255} 1" />
</material>
</visual>
</link>
<link name="board_link">
<visual>
<geometry>
<box size="0.200 ${WheelBase-WheelLen} ${0.150-0.08}"/>
</geometry>
<origin xyz="0 0 ${(0.150-0.08)/2}" rpy="0 0 0"/>
<material name="rgba(246,152,152,1)">
<color rgba="${246/255} ${152/255} ${152/255} 1" />
</material>
</visual>
</link>
<link name="camera" >
<visual>
<geometry>
<box size="0.015 0.045 0.03"/>
</geometry>
<origin xyz="0 0 ${(0.03)/2}" rpy="0 0 0"/>
<material name="rgba(93,78,78,1)">
<color rgba="${93/255} ${78/255} ${78/255} 1" />
</material>
</visual>
</link>
<joint name="camera_joint" type="fixed">
<parent link="base_link" />
<child link="camera" />
<origin xyz="${(0.276-0.015)/2-0.01} 0 ${0.08+0.03}" rpy="0 0 0" />
</joint>
<joint name="board_joint" type="fixed">
<parent link="base_link" />
<child link="board_link" />
<origin xyz="${-(0.276-0.2)/2} 0 ${0.08+0.03}" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
<link name="down_left_Link">
<visual>
<origin xyz="0 ${-WheelLen/2+0.025/2} 0" rpy="${-PI/2} 0 0" />
<geometry>
<cylinder radius="0.03" length="0.025" />
</geometry>
<material name="">
<color rgba="0.7450980392156863 0.12941176470588237 0.12941176470588237 1" />
</material>
</visual>
</link>
<joint name="down_left_joint" type="continuous">
<parent link="base_link" />
<child link="down_left_Link" />
<origin xyz="${-Track/2} ${WheelBase/2} ${0.03}" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
<link name="down_right_Link">
<visual>
<origin xyz="0 ${WheelLen/2-0.025/2} 0" rpy="${PI/2} 0 0" />
<geometry>
<cylinder radius="0.03" length="0.025"/>
</geometry>
<material name="">
<color rgba="0.7450980392156863 0.12941176470588237 0.12941176470588237 1" />
</material>
</visual>
</link>
<joint name="down_right_joint" type="continuous">
<parent link="base_link" />
<child link="down_right_Link" />
<origin xyz="${-Track/2} ${-WheelBase/2} ${0.03}" rpy="0 0 0" />
<axis xyz="0 -1 0" />
</joint>
<link name="up_left_Link">
<visual>
<origin xyz="0 ${-WheelLen/2+0.025/2} 0" rpy="${-PI/2} 0 0" />
<geometry>
<cylinder radius="0.03" length="0.025"/>
</geometry>
<material name="">
<color rgba="0.7450980392156863 0.12941176470588237 0.12941176470588237 1" />
</material>
</visual>
</link>
<joint name="up_left_joint" type="continuous">
<parent link="base_link" />
<child link="up_left_Link" />
<origin xyz="${Track/2} ${WheelBase/2} ${0.03}" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
<link name="up_right_Link">
<visual>
<origin xyz="0 ${WheelLen/2-0.025/2} 0" rpy="${PI/2} 0 0" />
<geometry>
<cylinder radius="0.03" length="0.025"/>
</geometry>
<material name="">
<color rgba="0.7450980392156863 0.12941176470588237 0.12941176470588237 1" />
</material>
</visual>
</link>
<joint name="up_right_joint" type="continuous">
<parent link="base_link" />
<child link="up_right_Link" />
<origin xyz="${Track/2} ${-WheelBase/2} ${0.03}" rpy="0 0 0" />
<axis xyz="0 -1 0" />
</joint>
</robot>

626
src/origincar_description/world/fishbot.world Normal file
View File

@@ -0,0 +1,626 @@
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<state world_name='default'>
<sim_time>51 251000000</sim_time>
<real_time>51 570048324</real_time>
<wall_time>1767706562 783975173</wall_time>
<iterations>51251</iterations>
<model name='fishbot_0'>
<pose>0.005469 0.07101 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='Wall_10'>
<pose>2.33047 -1.27133 0 0 0 -2.0944</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_11'>
<pose>0.005469 -2.61367 0 0 -0 3.14159</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_12'>
<pose>-2.31953 -1.27133 0 0 -0 2.0944</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_7'>
<pose>-2.31953 1.41335 0 0 -0 1.0472</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_8'>
<pose>0.005469 2.75569 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_9'>
<pose>2.33047 1.41335 0 0 0 -1.0472</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='unit_box'>
<pose>-0.039643 1.36341 0.499995 0 1e-05 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-0.039643 1.36341 0.499995 0 1e-05 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0.010615 -0.006191 -9.78231 0.012424 0.021225 1.8e-05</acceleration>
<wrench>0.010615 -0.006191 -9.78231 0 -0 0</wrench>
</link>
</model>
<model name='unit_cylinder'>
<pose>-1.0625 -0.503729 0.499993 -3e-06 -4e-06 -0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-1.0625 -0.503729 0.499993 -3e-06 -4e-06 -0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 -9.8 0 -0 0</acceleration>
<wrench>0 0 -9.8 0 -0 0</wrench>
</link>
</model>
<model name='unit_cylinder_0'>
<pose>0.724857 -0.542454 0.499997 3e-06 4e-06 -0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0.724857 -0.542454 0.499997 3e-06 4e-06 -0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 -9.8 0 -0 0</acceleration>
<wrench>0 0 -9.8 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<model name='fishbot_0'>
<pose>0.005469 0.07101 0 0 -0 0</pose>
<link name='Wall_10'>
<collision name='Wall_10_Collision'>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_10_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>0.996078 0.47451 0.0196078 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>2.325 -1.34234 0 0 -0 -2.0944</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_11'>
<collision name='Wall_11_Collision'>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_11_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>0.996078 0.47451 0.0196078 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-0 -2.68468 0 0 -0 3.14159</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_12'>
<collision name='Wall_12_Collision'>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_12_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>0.996078 0.47451 0.0196078 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-2.325 -1.34234 0 0 -0 2.0944</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_7'>
<collision name='Wall_7_Collision'>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_7_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>0.996078 0.47451 0.0196078 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-2.325 1.34234 0 0 -0 1.0472</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_8'>
<collision name='Wall_8_Collision'>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_8_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>0.996078 0.47451 0.0196078 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-0 2.68468 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_9'>
<collision name='Wall_9_Collision'>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_9_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>0.996078 0.47451 0.0196078 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>2.325 1.34234 0 0 -0 -1.0472</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<static>1</static>
</model>
<model name='unit_box'>
<pose>-0.039648 1.36341 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='unit_cylinder'>
<pose>-1.0625 -0.503731 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='unit_cylinder_0'>
<pose>0.724855 -0.542452 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>11.8556 -5.21446 33.9466 0 1.2498 2.56501</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>

893
src/origincar_description/world/gc_world.world Normal file
View File

@@ -0,0 +1,893 @@
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='aaaaaa.world'>
<pose>-4.71646 -0.134166 0 0 -0 0</pose>
<link name='Wall_11'>
<collision name='Wall_11_Collision'>
<geometry>
<box>
<size>6.5 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_11_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>6.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-10.5912 -3.10164 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_22'>
<collision name='Wall_22_Collision'>
<geometry>
<box>
<size>16 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_22_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>16 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-5.36881 -0.250968 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_23'>
<collision name='Wall_23_Collision'>
<geometry>
<box>
<size>19.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_23_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>19.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>4.18119 -8.17597 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_24'>
<collision name='Wall_24_Collision'>
<geometry>
<box>
<size>16.5 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_24_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>16.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>13.7312 -0.000968 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_25'>
<collision name='Wall_25_Collision'>
<geometry>
<box>
<size>19.2565 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_25_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>19.2565 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>4.18119 7.92403 0 0 -0 -3.11542</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_28'>
<collision name='Wall_28_Collision'>
<geometry>
<box>
<size>10.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_28_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>10.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-1.01381 3.08403 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_29'>
<collision name='Wall_29_Collision'>
<geometry>
<box>
<size>5.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_29_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>5.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>4.28619 0.534032 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_31'>
<collision name='Wall_31_Collision'>
<geometry>
<box>
<size>5 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_31_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-3.07881 -1.59597 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_33'>
<collision name='Wall_33_Collision'>
<geometry>
<box>
<size>5 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_33_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>9.25619 -5.56097 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_35'>
<collision name='Wall_35_Collision'>
<geometry>
<box>
<size>4.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_35_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>11.4662 3.66903 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<static>1</static>
</model>
<model name='unit_box'>
<pose>-6.35985 -5.47044 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='unit_cylinder'>
<pose>-2.08128 -5.46626 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='unit_cylinder_0'>
<pose>3.24338 0.112284 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='unit_box_0'>
<pose>1.72376 5.13 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<state world_name='default'>
<sim_time>148 245000000</sim_time>
<real_time>149 497524618</real_time>
<wall_time>1770372561 596283813</wall_time>
<iterations>148245</iterations>
<model name='aaaaaa.world'>
<pose>-4.71646 -0.134166 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='Wall_11'>
<pose>-15.3077 -3.23581 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_22'>
<pose>-10.0853 -0.385134 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_23'>
<pose>-0.53527 -8.31014 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_24'>
<pose>9.01474 -0.135134 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_25'>
<pose>-0.53527 7.78986 0 0 0 -3.11542</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_28'>
<pose>-5.73027 2.94986 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_29'>
<pose>-0.43027 0.399866 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_31'>
<pose>-7.79527 -1.73014 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_33'>
<pose>4.53973 -5.69514 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_35'>
<pose>6.74974 3.53486 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='unit_box'>
<pose>-6.35985 -5.47044 0.499995 -1e-05 -0 -0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-6.35985 -5.47044 0.499995 -1e-05 -0 -0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0.004709 0.011055 -9.78158 -0.022108 0.009414 1e-06</acceleration>
<wrench>0.004709 0.011055 -9.78158 0 -0 0</wrench>
</link>
</model>
<model name='unit_box_0'>
<pose>1.72376 5.13 0.5 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>1.72376 5.13 0.5 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-0.004709 -9.78112 9.78158 0.712677 -0.009414 -4.3e-05</acceleration>
<wrench>-0.004709 -9.78112 9.78158 0 -0 0</wrench>
</link>
</model>
<model name='unit_cylinder'>
<pose>-2.08128 -5.46626 0.499997 3e-06 4e-06 -0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-2.08128 -5.46626 0.499997 3e-06 4e-06 -0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 -9.8 0 -0 0</acceleration>
<wrench>0 0 -9.8 0 -0 0</wrench>
</link>
</model>
<model name='unit_cylinder_0'>
<pose>3.24338 0.112286 0.499993 -3e-06 -4e-06 -0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>3.24338 0.112286 0.499993 -3e-06 -4e-06 -0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 -9.8 0 -0 0</acceleration>
<wrench>0 0 -9.8 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>7.40665 -3.57041 31.6489 0 1.4618 3.1242</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>

828
src/origincar_description/world/test.world Normal file
View File

@@ -0,0 +1,828 @@
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='aaav.world'>
<pose>2.92199 0.94347 0 0 -0 0</pose>
<link name='Wall_0'>
<collision name='Wall_0_Collision'>
<geometry>
<box>
<size>5.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_0_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>5.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-4.8877 -4.19109 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_1'>
<collision name='Wall_1_Collision'>
<geometry>
<box>
<size>10 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_1_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>10 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>0.0373 -6.74109 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_10'>
<collision name='Wall_10_Collision'>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_10_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>1.06397 5.19109 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_12'>
<collision name='Wall_12_Collision'>
<geometry>
<box>
<size>3.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_12_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>1.09029 -4.75661 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_2'>
<collision name='Wall_2_Collision'>
<geometry>
<box>
<size>13.5 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_2_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>13.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>4.9623 -0.066086 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_3'>
<collision name='Wall_3_Collision'>
<geometry>
<box>
<size>10 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_3_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>10 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>0.0373 6.60891 0 0 -0 3.14159</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_4'>
<collision name='Wall_4_Collision'>
<geometry>
<box>
<size>9.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_4_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>9.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-4.8877 1.80891 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_6'>
<collision name='Wall_6_Collision'>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_6_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-3.4123 3.54932 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_7'>
<collision name='Wall_7_Collision'>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_7_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-1.8623 1.99932 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_8'>
<collision name='Wall_8_Collision'>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_8_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-0.312301 0.449317 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<static>1</static>
</model>
<model name='unit_box'>
<pose>0.15442 -3.50156 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='unit_cylinder'>
<pose>1.94616 -0.907636 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='unit_box_0'>
<pose>5.80896 0.105775 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<state world_name='default'>
<sim_time>77 768000000</sim_time>
<real_time>78 15142337</real_time>
<wall_time>1770533307 91681029</wall_time>
<iterations>77768</iterations>
<model name='aaav.world'>
<pose>2.92199 0.94347 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='Wall_0'>
<pose>-1.96571 -3.24762 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_1'>
<pose>2.95929 -5.79762 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_10'>
<pose>3.98596 6.13456 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_12'>
<pose>4.01228 -3.81314 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_2'>
<pose>7.88429 0.877384 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_3'>
<pose>2.95929 7.55238 0 0 -0 3.14159</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_4'>
<pose>-1.96571 2.75238 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_6'>
<pose>-0.49031 4.49279 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_7'>
<pose>1.05969 2.94279 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_8'>
<pose>2.60969 1.39279 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='unit_box'>
<pose>0.15442 -3.50156 0.5 0 -0 0.691942</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0.15442 -3.50156 0.5 0 -0 0.691942</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-0 0 -0 0 -0 0</acceleration>
<wrench>-0 0 -0 0 -0 0</wrench>
</link>
</model>
<model name='unit_box_0'>
<pose>5.80896 0.105775 0.5 0 -0 0.665093</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>5.80896 0.105775 0.5 0 -0 0.665093</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>7.28216 6.1355 9.84639 -2.8462 1.14378 -3.14153</acceleration>
<wrench>7.28216 6.1355 9.84639 0 -0 0</wrench>
</link>
</model>
<model name='unit_cylinder'>
<pose>1.94616 -0.907634 0.499993 -3e-06 -3e-06 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>1.94616 -0.907634 0.499993 -3e-06 -3e-06 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 -9.8 0 -0 0</acceleration>
<wrench>0 0 -9.8 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>4.60766 -4.56851 22.292 -0 1.35564 1.44419</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>