diff --git a/config/adc_car_global_ekf.yaml b/config/adc_car_global_ekf.yaml
index ac46ea08092bdd9e12beca31cb89b6eac06948c6..e00747fd90751f74a48fcfe7c17f3b2d6f64e35f 100644
--- a/config/adc_car_global_ekf.yaml
+++ b/config/adc_car_global_ekf.yaml
@@ -20,25 +20,25 @@ odom0_config: [false, false, false,
false, false, true,
false, false, false]
-imu0: /adc_car/sensors/imu_enu
-imu0_config: [false, false, false,
- false, false, true,
- false, false, false,
- false, false, true,
- true, false, false]
+#imu0: /adc_car/sensors/imu_enu
+#imu0_config: [false, false, false,
+# false, false, false,
+# false, false, false,
+# false, false, true,
+# true, false, false]
-imu0_differential: false
-imu0_relative: false
-imu0_remove_gravitational_acceleration: true
+#imu0_differential: false
+#imu0_relative: true
+#imu0_remove_gravitational_acceleration: true
pose0: /adc_car/amcl_pose
-pose0_config: [true, true, false,
- false, false, true,
+pose0_config: [true, true, true,
+ false, false, false,
false, false, false,
false, false, false,
false, false, false]
pose0_differential: false
-#pose0_relative: false
+pose0_relative: false
#pose0_queue_size: 5
#pose0_rejection_threshold: 2 # Note the difference in parameter name
#pose0_nodelay: false
diff --git a/config/adc_car_local_ekf.yaml b/config/adc_car_local_ekf.yaml
index d10847e75b7dabcd1a1c4a38c6a94d9f3336ce6c..9b52c7950ee062e36ba9667bd37e2779d9957a0a 100644
--- a/config/adc_car_local_ekf.yaml
+++ b/config/adc_car_local_ekf.yaml
@@ -6,6 +6,7 @@ print_diagnostics: true
debug: false
debug_out_file: /tmp/debug_ekf_localization.txt
use_control: false
+control_config: [true, false, false, false, false, true]
publish_tf: true
map_frame: map
@@ -16,7 +17,7 @@ world_frame: adc_car/odom
odom0: /adc_car/odom
odom0_config: [false, false, false,
false, false, false,
- true, true, false,
+ true, false, false,
false, false, true,
false, false, false]
@@ -25,10 +26,10 @@ imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
- true, false, false]
+ false, false, false]
imu0_differential: false
-imu0_relative: false
+imu0_relative: true
imu0_remove_gravitational_acceleration: true
#meaning_config: [x, y, z,
diff --git a/config/adc_car_move_base/amcl_no_tf.yaml b/config/adc_car_move_base/amcl_no_tf.yaml
index 4eb0b2818ccfe65ff0f28ba68bb4063a8de2bbd3..a44c4ec720671a9e5a929d6d72235de8359b5fe4 100644
--- a/config/adc_car_move_base/amcl_no_tf.yaml
+++ b/config/adc_car_move_base/amcl_no_tf.yaml
@@ -7,12 +7,12 @@ initial_pose_y: 0.0
initial_pose_a: 0.0
# filer parameters
-min_particles: 100
+min_particles: 1000
max_particles: 5000
kld_err: 0.01
kld_z: 0.99
-update_min_d: 0.2
-update_min_a: 0.5236
+update_min_d: 0.1
+update_min_a: 0.1
resample_interval: 2
transform_tolerance: 0.1
recovery_alpha_slow: 0.0
@@ -40,7 +40,7 @@ laser_model_type: 'likelihood_field'
odom_model_type: "diff"
odom_alpha1: 0.2
odom_alpha2: 0.2
-odom_alpha3: 0.2
+odom_alpha3: 0.02
odom_alpha4: 0.2
odom_alpha5: 0.2
tf_broadcast: false
diff --git a/config/adc_car_move_base/global_planner/iri_opendrive_params.yaml b/config/adc_car_move_base/global_planner/iri_opendrive_params.yaml
index 6aaa42b8b930d49938fcd2b6ee5ecf953bd2c0fa..07238a3ba8557ca5dd70be6cf89bc5d31afd4afe 100644
--- a/config/adc_car_move_base/global_planner/iri_opendrive_params.yaml
+++ b/config/adc_car_move_base/global_planner/iri_opendrive_params.yaml
@@ -4,7 +4,7 @@ OpendriveGlobalPlanner:
dist_tol: 0.3
multi_hyp: True
resolution: 0.01
- scale_factor: 10.0
+ scale_factor: 8.0
min_road_length: 0.01
cost_type: 0
diff --git a/config/adc_car_move_base/global_planner/sbpl_params.yaml b/config/adc_car_move_base/global_planner/sbpl_params.yaml
deleted file mode 100644
index 4eb28a34d5964249572bb794f98a3a4fb7e94ca6..0000000000000000000000000000000000000000
--- a/config/adc_car_move_base/global_planner/sbpl_params.yaml
+++ /dev/null
@@ -1 +0,0 @@
-base_global_planner: "SBPLLatticePlanner"
diff --git a/config/adc_car_move_base/local_planner/ackermann_sbpl_params.yaml b/config/adc_car_move_base/local_planner/ackermann_sbpl_params.yaml
deleted file mode 100644
index 7826af5700280e05b9a4080c49bcb10193ea16ae..0000000000000000000000000000000000000000
--- a/config/adc_car_move_base/local_planner/ackermann_sbpl_params.yaml
+++ /dev/null
@@ -1,53 +0,0 @@
-base_local_planner: "AckermannPlannerROS"
-
-AckermannPlannerROS:
- max_sim_time: 3.0
- min_sim_time: 1.0
- sim_granularity: 0.2
- angular_sim_granularity: 0.02
-
- path_distance_bias: 50.0
- goal_distance_bias: 10.0
- occdist_scale: 0.05
-
-# stop_time_buffer:
- oscillation_reset_dist: 0.002
- oscillation_reset_angle: 0.001
-
-# forward_point_distance:
-
-# scaling_speed:
-# max_scaling_factor:
-
- trans_vel_samples: 31
- steer_angle_samples: 21
-
- prune_plan: true
- xy_goal_tolerance: 0.25
- yaw_goal_tolerance: 0.5
- trans_stopped_vel: 0.1
- rot_stopped_vel: 0.1
-
- restore_defaults: false
-
- max_trans_vel: 1.0
- min_trans_vel: -1.0
- max_trans_acc: 0.2
- max_steer_angle: 0.4
- min_steer_angle: -0.4
- max_steer_vel: 0.3
- min_steer_vel: -0.3
- max_steer_acc: 1.0
- axis_distance: 0.3662
- wheel_distance: 0.216
- wheel_radius: 0.05
-
- split_ignore_length: 0.1
-
- cmd_vel_avg: 1
- odom_avg: 1
-
- planner_patience: 5
-
- hdiff_scale: 0.5
- heading_points: 8
diff --git a/config/adc_car_odometry_params.yaml b/config/adc_car_odometry_params.yaml
deleted file mode 100644
index d200601124867b0a16760e0a72a97f6a6f7e84bf..0000000000000000000000000000000000000000
--- a/config/adc_car_odometry_params.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-rate: 40
-odom_frame: "adc_car/odom"
-robot_frame: "adc_car/base_footprint"
-encoder_ticks: 60
-wheel_diameter: 0.100
-wheel_distance: 0.216
-axel_distance: 0.3662
-filter_coeff: 0.75
-max_steer_angle: 0.4
-min_steer_angle: -0.4
-max_steer_control: 100.0
-min_steer_control: -100.0
-publish_tf: True
-
diff --git a/launch/adc_sim_env.launch b/launch/adc_sim_env.launch
index 69d6c9d1c3221ddf3806afaac2b9f890c13c5a12..8845a3c7a6f93e10f969f6fbf432c5e9ab08a3cd 100644
--- a/launch/adc_sim_env.launch
+++ b/launch/adc_sim_env.launch
@@ -29,9 +29,9 @@
<arg name="sim_config_path" value="$(find iri_adc_launch)/config/adc_car"/>
<arg name="car_controller_config_file" value="$(find iri_adc_launch)/config/adc_car_controller_config.yaml"/>
<arg name="control_config_file" value="$(find iri_adc_launch)/config/adc_car_control_params.yaml"/>
- <arg name="odometry_config_file" value="$(find iri_adc_launch)/config/adc_car_odometry_params.yaml"/>
+ <arg name="odometry_config_file" value="$(find iri_adc_launch)/config/adc_car_odometry_params_no_tf.yaml"/>
<arg name="cmd_vel_mux_config_file" value="$(find iri_adc_launch)/config/adc_car_mux.yaml"/>
- <arg name="standalone" value="True"/>
+ <arg name="local_ekf_config_file" value="$(find iri_adc_launch)/config//adc_car_local_ekf.yaml"/>
</include>
<include file="$(find iri_adc_launch)/launch/sample_signals.launch">
diff --git a/launch/sim_localization_global.launch b/launch/sim_localization_global.launch
deleted file mode 100644
index c6346f66e669bb338b21ccaf8ac893c0b8f362f3..0000000000000000000000000000000000000000
--- a/launch/sim_localization_global.launch
+++ /dev/null
@@ -1,81 +0,0 @@
-<?xml version="1.0"?>
-<!-- -->
-<launch>
-
- <arg name="name" default="adc_car"/>
- <arg name="circuit" default="sample"/>
- <arg name="map_name" default="$(arg circuit)"/>
- <arg name="output" default="screen"/>
- <arg name="launch_prefix" default=""/>
-
- <arg name="world" default="empty"/>
- <arg name="gazebo_gui" default="True"/>
- <arg name="car_x" default="0.0"/>
- <arg name="car_y" default="0.0"/>
- <arg name="car_yaw" default="0.0"/>
-
- <include file="$(find iri_model_car_gazebo)/launch/sim.launch">
- <arg name="name" value="$(arg name)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- <arg name="world" value="$(arg world)"/>
- <arg name="gazebo_gui" value="$(arg gazebo_gui)"/>
- <arg name="car_x" value="$(arg car_x)"/>
- <arg name="car_y" value="$(arg car_y)"/>
- <arg name="car_yaw" value="$(arg car_yaw)"/>
- <arg name="sim_config_path" value="$(find iri_adc_launch)/config/$(arg name)"/>
- <arg name="car_controller_config_file" value="$(find iri_adc_launch)/config/$(arg name)_controller_config.yaml"/>
- <arg name="control_config_file" value="$(find iri_adc_launch)/config/$(arg name)_control_params.yaml"/>
- <arg name="odometry_config_file" value="$(find iri_adc_launch)/config/$(arg name)_odometry_params_no_tf.yaml"/>
- <arg name="cmd_vel_mux_config_file" value="$(find iri_adc_launch)/config/$(arg name)_mux.yaml"/>
- <arg name="standalone" value="False"/>
- </include>
-
- <include file="$(find iri_model_car_rosnav)/launch/include/localization.launch">
- <arg name="name" value="$(arg name)"/>
- <arg name="local_ekf_yaml_file" default="$(find iri_adc_launch)/config/$(arg name)_local_ekf.yaml"/>
- <arg name="global_ekf_yaml_file" default="$(find iri_adc_launch)/config/$(arg name)_global_ekf.yaml"/>
- <arg name="global_loc" default="true"/>
- </include>
-
- <include file="$(find iri_adc_circuit_example)/launch/spawn.launch">
- <arg name="name" value="$(arg circuit)"/>
- </include>
-
- <include file="$(find iri_model_car_rosnav)/launch/nav_map.launch">
- <arg name="name" value="$(arg name)"/>
- <arg name="params_path" value="$(find iri_adc_launch)/config/$(arg name)_move_base"/>
- <arg name="move_base_params" value="move_base_params.yaml"/>
- <arg name="costmap_common_params" value="common_params.yaml"/>
- <arg name="costmap_local_params" value="local_params.yaml"/>
- <arg name="costmap_global_params" value="global_params.yaml"/>
- <arg name="local_planner" value="ackermann_sbpl"/>
- <arg name="global_planner" value="sbpl"/>
- <arg name="odom_topic" value="/$(arg name)/local_odom_combined"/>
- <arg name="map_path" value="$(find iri_adc_circuit_example)/map"/>
- <arg name="map_name" value="$(arg map_name)"/>
- <arg name="amcl_config" value="$(find iri_adc_launch)/config/$(arg name)_move_base/amcl_no_tf.yaml"/>
- <arg name="initial_x" value="$(arg car_x)"/>
- <arg name="initial_y" value="$(arg car_y)"/>
- <arg name="initial_yaw" value="$(arg car_yaw)"/>
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="opendrive_file_path" default="$(find iri_adc_circuit_example)/data"/>
- <arg name="opendrive_file_name" default="$(arg circuit)"/>
- </include>
-
- <include file="$(find iri_teleop_launch)/launch/rqt_teleop.launch">
- <arg name="ns" value="$(arg name)"/>
- <arg name="twist_topic" value="/$(arg name)/teleop/cmd_vel"/>
- <arg name="config_file" value="$(find iri_teleop_launch)/config/rqt_teleop.yaml" />
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- </include>
-
- <node name="rviz"
- pkg="rviz"
- type="rviz"
- args="-d $(find iri_adc_launch)/rviz/localization_global.rviz">
- </node>
-
-</launch>
diff --git a/launch/sim_localization_local.launch b/launch/sim_localization_local.launch
deleted file mode 100644
index ae3c50c8a26cde599c6a9169bc005c465640ec7d..0000000000000000000000000000000000000000
--- a/launch/sim_localization_local.launch
+++ /dev/null
@@ -1,58 +0,0 @@
-<?xml version="1.0"?>
-<!-- -->
-<launch>
-
- <arg name="name" default="adc_car"/>
- <arg name="output" default="screen"/>
- <arg name="launch_prefix" default=""/>
-
-
-
- <arg name="world" default="empty"/>
- <arg name="gazebo_gui" default="True"/>
- <arg name="car_x" default="0.0"/>
- <arg name="car_y" default="0.0"/>
- <arg name="car_yaw" default="0.0"/>
-
- <include file="$(find iri_model_car_gazebo)/launch/sim.launch">
- <arg name="name" value="$(arg name)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- <arg name="world" value="$(arg world)"/>
- <arg name="gazebo_gui" value="$(arg gazebo_gui)"/>
- <arg name="car_x" value="$(arg car_x)"/>
- <arg name="car_y" value="$(arg car_y)"/>
- <arg name="car_yaw" value="$(arg car_yaw)"/>
- <arg name="sim_config_path" value="$(find iri_adc_launch)/config/$(arg name)"/>
- <arg name="car_controller_config_file" value="$(find iri_adc_launch)/config/$(arg name)_controller_config.yaml"/>
- <arg name="control_config_file" value="$(find iri_adc_launch)/config/$(arg name)_control_params.yaml"/>
- <arg name="odometry_config_file" value="$(find iri_adc_launch)/config/$(arg name)_odometry_params_no_tf.yaml"/>
- <arg name="cmd_vel_mux_config_file" value="$(find iri_adc_launch)/config/$(arg name)_mux.yaml"/>
- <arg name="standalone" value="True"/>
- </include>
-
- <include file="$(find iri_model_car_rosnav)/launch/include/localization.launch">
- <arg name="name" value="$(arg name)"/>
- <arg name="local_ekf_yaml_file" default="$(find iri_adc_launch)/config/$(arg name)_local_ekf.yaml"/>
- <arg name="global_ekf_yaml_file" default="$(find iri_adc_launch)/config/$(arg name)_global_ekf.yaml"/>
- <arg name="global_loc" default="false"/>
- </include>
-
-<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="static_tf_map_odom"
- args="0 0 0 0 0 0 map $(arg name)/odom"/>-->
-
- <include file="$(find iri_teleop_launch)/launch/rqt_teleop.launch">
- <arg name="ns" value="$(arg name)"/>
- <arg name="twist_topic" value="/$(arg name)/teleop/cmd_vel"/>
- <arg name="config_file" value="$(find iri_teleop_launch)/config/rqt_teleop.yaml" />
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- </include>
-
- <node name="rviz"
- pkg="rviz"
- type="rviz"
- args="-d $(find iri_adc_launch)/rviz/localization_local.rviz">
- </node>
-
-</launch>
diff --git a/launch/sim_navigation.launch b/launch/sim_navigation.launch
index bef8360a2cd235cb1b5c31c7631e8488960dcd29..3cc5c2ec55d40cbbb070cc5a4060ed47570b1680 100644
--- a/launch/sim_navigation.launch
+++ b/launch/sim_navigation.launch
@@ -3,80 +3,55 @@
<launch>
<arg name="name" default="adc_car"/>
- <arg name="circuit" default="sample"/>
- <arg name="map_name" default="$(arg circuit)"/>
- <arg name="global_planner" default="sbpl"/>
<arg name="output" default="screen"/>
<arg name="launch_prefix" default=""/>
+ <arg name="map_path" default="$(find iri_adc_circuit_example)/map"/>
+ <arg name="map_name" default="sample" />
+ <arg name="opendrive_file_path" default="$(find iri_adc_circuit_example)/data"/>
+ <arg name="opendrive_file_name" default="sample"/>
- <arg name="world" default="empty"/>
<arg name="gazebo_gui" default="True"/>
<arg name="car_x" default="0.0"/>
<arg name="car_y" default="-0.22"/>
<arg name="car_yaw" default="0.0"/>
- <include file="$(find iri_model_car_gazebo)/launch/sim.launch">
- <arg name="name" value="$(arg name)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- <arg name="world" value="$(arg world)"/>
- <arg name="gazebo_gui" value="$(arg gazebo_gui)"/>
- <arg name="car_x" value="$(arg car_x)"/>
- <arg name="car_y" value="$(arg car_y)"/>
- <arg name="car_yaw" value="$(arg car_yaw)"/>
- <arg name="sim_config_path" value="$(find iri_adc_launch)/config/$(arg name)"/>
- <arg name="car_controller_config_file" value="$(find iri_adc_launch)/config/$(arg name)_controller_config.yaml"/>
- <arg name="control_config_file" value="$(find iri_adc_launch)/config/$(arg name)_control_params.yaml"/>
- <arg name="odometry_config_file" value="$(find iri_adc_launch)/config/$(arg name)_odometry_params_no_tf.yaml"/>
- <arg name="cmd_vel_mux_config_file" value="$(find iri_adc_launch)/config/$(arg name)_mux.yaml"/>
- <arg name="standalone" value="False"/>
+ <include file="$(find iri_rosnav)/launch/nav.launch">
+ <arg name="ns" value="$(arg name)"/>
+ <arg name="path" value="$(find iri_adc_launch)/config/$(arg name)_move_base"/>
+ <arg name="move_base_params" value="move_base_params.yaml"/>
+ <arg name="costmap_common_params" value="common_params.yaml"/>
+ <arg name="costmap_local_params" value="local_params.yaml"/>
+ <arg name="costmap_global_params" value="global_params.yaml"/>
+ <arg name="map_frame_id" value="map"/>
+ <arg name="odom_frame_id" value="$(arg name)/odom"/>
+ <arg name="base_frame_id" value="$(arg name)/base_footprint"/>
+ <arg name="map_topic" value="/$(arg name)/map"/>
+ <arg name="map_service" value="/$(arg name)/static_map"/>
+ <arg name="odom_topic" value="/$(arg name)/local_odom_combined"/>
+ <arg name="cmd_vel_topic" value="/$(arg name)/navigation/cmd_vel"/>
+ <arg name="scan_topic" value="/$(arg name)/sensors/scan"/>
+ <arg name="use_map" value="true"/>
+ <arg name="use_map_server" value="true"/>
+ <arg name="map_path" value="$(arg map_path)"/>
+ <arg name="map_name" value="$(arg map_name)"/>
+ <arg name="use_amcl" value="true"/>
+ <arg name="amcl_config" value="$(find iri_model_car_rosnav)/params/amcl_no_tf.yaml"/>
+ <arg name="initial_x" value="$(arg car_x)"/>
+ <arg name="initial_y" value="$(arg car_y)"/>
+ <arg name="initial_yaw" value="$(arg car_yaw)"/>
+ <arg name="use_fake_loc" value="false"/>
+ <arg name="use_gmapping" value="false"/>
+ <arg name="local_planner" value="ackermann"/>
+ <arg name="global_planner" value="iri_opendrive"/>
+ <arg name="output" value="$(arg output)" />
+ <arg name="launch_prefix" value="$(arg launch_prefix)" />
</include>
<include file="$(find iri_model_car_rosnav)/launch/include/localization.launch">
- <arg name="name" value="$(arg name)"/>
- <arg name="local_ekf_yaml_file" default="$(find iri_adc_launch)/config/$(arg name)_local_ekf.yaml"/>
- <arg name="global_ekf_yaml_file" default="$(find iri_adc_launch)/config/$(arg name)_global_ekf.yaml"/>
- <arg name="global_loc" default="true"/>
+ <arg name="name" value="$(arg name)"/>
+ <arg name="global_ekf_config_file" value="$(find iri_adc_launch)/config/adc_car_global_ekf.yaml"/>
</include>
- <include file="$(find iri_adc_circuit_example)/launch/spawn.launch">
- <arg name="name" value="$(arg circuit)"/>
- </include>
-
- <include file="$(find iri_model_car_rosnav)/launch/nav_map.launch">
- <arg name="name" value="$(arg name)"/>
- <arg name="params_path" value="$(find iri_adc_launch)/config/$(arg name)_move_base"/>
- <arg name="move_base_params" value="move_base_params.yaml"/>
- <arg name="costmap_common_params" value="common_params.yaml"/>
- <arg name="costmap_local_params" value="local_params.yaml"/>
- <arg name="costmap_global_params" value="global_params.yaml"/>
- <arg name="local_planner" value="ackermann_sbpl"/>
- <arg name="global_planner" value="$(arg global_planner)"/>
- <arg name="odom_topic" value="/$(arg name)/local_odom_combined"/>
- <arg name="map_path" value="$(find iri_adc_circuit_example)/map"/>
- <arg name="map_name" value="$(arg map_name)"/>
- <arg name="amcl_config" value="$(find iri_adc_launch)/config/$(arg name)_move_base/amcl_no_tf.yaml"/>
- <arg name="initial_x" value="$(arg car_x)"/>
- <arg name="initial_y" value="$(arg car_y)"/>
- <arg name="initial_yaw" value="$(arg car_yaw)"/>
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="opendrive_file_path" default="$(find iri_adc_circuit_example)/data"/>
- <arg name="opendrive_file_name" default="$(arg circuit)"/>
- </include>
-
- <include file="$(find iri_teleop_launch)/launch/rqt_teleop.launch">
- <arg name="ns" value="$(arg name)"/>
- <arg name="twist_topic" value="/$(arg name)/teleop/cmd_vel"/>
- <arg name="config_file" value="$(find iri_teleop_launch)/config/rqt_teleop.yaml" />
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- </include>
-
- <node name="rviz"
- pkg="rviz"
- type="rviz"
- args="-d $(find iri_adc_launch)/rviz/nav.rviz">
- </node>
+ <param name="/$(arg name)/move_base/OpendriveGlobalPlanner/opendrive_file" value="$(arg opendrive_file_path)/$(arg opendrive_file_name).xodr" />
</launch>