diff --git a/launch/nav_map.launch b/launch/nav_map.launch
index 8a41bb0234814f8132e1743c546f0ba40cbacc0a..ad978304b02cc7ddaedf9114b8cb3294b8609aab 100644
--- a/launch/nav_map.launch
+++ b/launch/nav_map.launch
@@ -2,7 +2,7 @@
<!-- -->
<launch>
- <arg name="rviz" default="true"/>
+ <arg name="rviz" default="false"/>
<arg name="ns" default="ana"/>
<arg name="pkg" default="iri_ana_rosnav"/>
<arg name="path" default="$(eval find(arg('pkg')))"/>
@@ -69,15 +69,17 @@
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
</include>
-
- <include file="$(find iri_ana_rosnav)/launch/include/pointcloud_to_laserscan.launch">
+
+ <include file="$(find iri_rosnav)/launch/include/pointcloud_to_laserscan.launch">
<arg name="ns" value="$(arg ns)"/>
+ <arg name="node_name" value="pcl_to_laserscan"/>
+ <arg name="config_file" value="$(find iri_ana_rosnav)/params/pointcloud_to_laserscan.yaml" />
<arg name="scan_topic" value="$(arg scan_topic)"/>
<arg name="cloud_topic" value="/$(arg ns)/sensors/velodyne_points"/>
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
</include>
-
+
<node name="rviz"
pkg="rviz"
type="rviz"
diff --git a/launch/nav_mapping.launch b/launch/nav_mapping.launch
index 9925423ff411a6fb9a056c9c2e47a84da249262d..1abb83c88bb5d60051cb48a6e1382fc89b9ed8ab 100644
--- a/launch/nav_mapping.launch
+++ b/launch/nav_mapping.launch
@@ -69,15 +69,17 @@
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
</include>
-
- <include file="$(find iri_ana_rosnav)/launch/include/pointcloud_to_laserscan.launch">
+
+ <include file="$(find iri_rosnav)/launch/include/pointcloud_to_laserscan.launch">
<arg name="ns" value="$(arg ns)"/>
+ <arg name="node_name" value="pcl_to_laserscan"/>
+ <arg name="config_file" value="$(find iri_ana_rosnav)/params/pointcloud_to_laserscan.yaml" />
<arg name="scan_topic" value="$(arg scan_topic)"/>
<arg name="cloud_topic" value="/$(arg ns)/sensors/velodyne_points"/>
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
</include>
-
+
<node name="rviz"
pkg="rviz"
type="rviz"
diff --git a/launch/nav_nomap.launch b/launch/nav_nomap.launch
index 3b4e9ebab8149c78b8805cf7404504d7ba732d3f..d294ba6ec288d537a0da668b56d4e92a540ec3bc 100644
--- a/launch/nav_nomap.launch
+++ b/launch/nav_nomap.launch
@@ -69,15 +69,17 @@
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
</include>
-
- <include file="$(find iri_ana_rosnav)/launch/include/pointcloud_to_laserscan.launch">
+
+ <include file="$(find iri_rosnav)/launch/include/pointcloud_to_laserscan.launch">
<arg name="ns" value="$(arg ns)"/>
+ <arg name="node_name" value="pcl_to_laserscan"/>
+ <arg name="config_file" value="$(find iri_ana_rosnav)/params/pointcloud_to_laserscan.yaml" />
<arg name="scan_topic" value="$(arg scan_topic)"/>
<arg name="cloud_topic" value="/$(arg ns)/sensors/velodyne_points"/>
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
</include>
-
+
<node name="rviz"
pkg="rviz"
type="rviz"
diff --git a/params/amcl.yaml b/params/amcl.yaml
index e72b96063dde8595683ad4ea7b95a1ea9b128c3b..7d770bec4c12d388f42399baa9627d899ce7bf00 100644
--- a/params/amcl.yaml
+++ b/params/amcl.yaml
@@ -10,12 +10,12 @@ min_particles: 100
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
-recovery_alpha_fast: 0.0
+recovery_alpha_slow: 0.1
+recovery_alpha_fast: 0.5
initial_cov_xx: 0.25
initial_cov_yy: 0.25
initial_cov_aa: 0.0685
@@ -37,8 +37,8 @@ laser_likelihood_max_dist: 2.0
laser_model_type: 'likelihood_field'
# odom parameters
odom_model_type: "diff"
-odom_alpha1: 0.2
-odom_alpha2: 0.2
+odom_alpha1: 5.0
+odom_alpha2: 5.0
odom_alpha3: 0.2
odom_alpha4: 0.2
odom_alpha5: 0.2
diff --git a/params/costmap/common_params.yaml b/params/costmap/common_params.yaml
index 2ddbeb0c4e181fa59f3243b84c0f9e1b52f943cd..c49dce7e00778c8d25275bf969981d9d548fb21d 100755
--- a/params/costmap/common_params.yaml
+++ b/params/costmap/common_params.yaml
@@ -1,5 +1,6 @@
# footprint parameters
-footprint: [[0.35, 0.35], [0.35, -0.35], [-0.35,-0.35 ], [-0.35, 0.35]]
+#footprint: [[0.35, 0.35], [0.35, -0.35], [-0.35,-0.35 ], [-0.35, 0.35]]
+footprint: [[0.3, 0.3], [0.3, -0.3], [-0.3,-0.3 ], [-0.3, 0.3]]
#robot_radius: 0.35
footprint_padding: 0.01
footprint_topic: /ana/footprint
@@ -57,4 +58,4 @@ inflation_layer:
inflate_unknown: false
inflation_radius: 2.0
cost_scaling_factor: 5.0
- inflation_radius: 0.5
\ No newline at end of file
+ inflation_radius: 0.5
diff --git a/params/costmap/local_params.yaml b/params/costmap/local_params.yaml
index b572e5558c406e14683a712e6ca3323d02581001..c84aace3d41834c16ba1414f6414e7ae69632f92 100755
--- a/params/costmap/local_params.yaml
+++ b/params/costmap/local_params.yaml
@@ -12,4 +12,4 @@ origin_y: 0.0
#costmap plugins
plugins:
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
\ No newline at end of file
+ - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
diff --git a/params/costmap/no_map/global_params.yaml b/params/costmap/no_map/global_params.yaml
index 88305431f41ff9d0d8bfbdc5008f86004587a1ca..bbb4b4ccbbc1afd20b2d9109d253d0cc8ad224c4 100644
--- a/params/costmap/no_map/global_params.yaml
+++ b/params/costmap/no_map/global_params.yaml
@@ -12,4 +12,4 @@ origin_y: 0.0
plugins:
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
- - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
\ No newline at end of file
+ - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
diff --git a/params/dwa/planner_params.yaml b/params/dwa/planner_params.yaml
index 0ebde63302f4db064b5839a2ce15bf894cdf6841..5b6869f0af997bd33148260eaddcf2e82b313f6b 100644
--- a/params/dwa/planner_params.yaml
+++ b/params/dwa/planner_params.yaml
@@ -9,7 +9,7 @@ DWAPlannerROS:
min_vel_x: -0.5
max_vel_y: 0.0
min_vel_y: 0.0
- max_rot_vel: 2.0
+ max_rot_vel: 0.5
min_rot_vel: 0.0
acc_lim_theta: 1.0
acc_lim_x: 1.0
diff --git a/params/local_planner/dwa_params.yaml b/params/local_planner/dwa_params.yaml
index a13f9db6c72757bb3fe13342da73db244d755a8a..b1258e3d02ff9b2a627af291e283150d607dc74c 100644
--- a/params/local_planner/dwa_params.yaml
+++ b/params/local_planner/dwa_params.yaml
@@ -10,7 +10,7 @@ DWAPlannerROS:
max_vel_y: 0.0
min_vel_y: 0.0
max_rot_vel: 2.0
- min_rot_vel: -2.0
+ min_rot_vel: 0.0
acc_lim_theta: 5.0
acc_lim_x: 1.0
acc_lim_y: 0.0
diff --git a/rviz/ana.rviz b/rviz/ana.rviz
index a748735afd4a90337a21a5693001d45d0c6ea40a..753c4395a17cdb9083758b84845ed54d753394f1 100644
--- a/rviz/ana.rviz
+++ b/rviz/ana.rviz
@@ -6,8 +6,9 @@ Panels:
Expanded:
- /Sensors1
- /Nav1
+ - /People1
Splitter Ratio: 0.5
- Tree Height: 525
+ Tree Height: 418
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -98,27 +99,27 @@ Visualization Manager:
Alpha: 1
Show Axes: false
Show Trail: false
- camera_left_ir_frame:
+ camera_infra1_frame:
Alpha: 1
Show Axes: false
Show Trail: false
- camera_left_ir_optical_frame:
+ camera_infra1_optical_frame:
Alpha: 1
Show Axes: false
Show Trail: false
- camera_link:
+ camera_infra2_frame:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- camera_right_ir_frame:
+ camera_infra2_optical_frame:
Alpha: 1
Show Axes: false
Show Trail: false
- camera_right_ir_optical_frame:
+ camera_link:
Alpha: 1
Show Axes: false
Show Trail: false
+ Value: true
front_left_axle:
Alpha: 1
Show Axes: false
@@ -230,8 +231,14 @@ Visualization Manager:
Value: true
ana/base_footprint:
Value: true
+ ana/base_footprint_robot:
+ Value: true
ana/base_link:
Value: true
+ ana/camera_aligned_depth_to_color_frame:
+ Value: true
+ ana/camera_aligned_depth_to_infra1_frame:
+ Value: true
ana/camera_bottom_screw_frame:
Value: true
ana/camera_color_frame:
@@ -242,15 +249,15 @@ Visualization Manager:
Value: true
ana/camera_depth_optical_frame:
Value: true
- ana/camera_left_ir_frame:
+ ana/camera_infra1_frame:
Value: true
- ana/camera_left_ir_optical_frame:
+ ana/camera_infra1_optical_frame:
Value: true
- ana/camera_link:
+ ana/camera_infra2_frame:
Value: true
- ana/camera_right_ir_frame:
+ ana/camera_infra2_optical_frame:
Value: true
- ana/camera_right_ir_optical_frame:
+ ana/camera_link:
Value: true
ana/front_left_axle:
Value: true
@@ -292,9 +299,15 @@ Visualization Manager:
Value: true
ana/velodyne_base:
Value: true
- map:
+ camera_accel_frame:
+ Value: true
+ camera_accel_optical_frame:
+ Value: true
+ camera_gyro_frame:
Value: true
- world:
+ camera_gyro_optical_frame:
+ Value: true
+ map:
Value: true
Marker Scale: 0.100000001
Name: TF
@@ -332,26 +345,36 @@ Visualization Manager:
ana/ana_realsense_support:
ana/camera_bottom_screw_frame:
ana/camera_link:
+ ana/camera_aligned_depth_to_color_frame:
+ ana/camera_color_optical_frame:
+ {}
+ ana/camera_aligned_depth_to_infra1_frame:
+ ana/camera_infra1_optical_frame:
+ {}
+ ana/camera_color_frame:
+ {}
ana/camera_depth_frame:
- ana/camera_color_frame:
- ana/camera_color_optical_frame:
- {}
ana/camera_depth_optical_frame:
{}
- ana/camera_left_ir_frame:
- ana/camera_left_ir_optical_frame:
- {}
- ana/camera_right_ir_frame:
- ana/camera_right_ir_optical_frame:
- {}
+ ana/camera_infra1_frame:
+ {}
+ ana/camera_infra2_frame:
+ ana/camera_infra2_optical_frame:
+ {}
+ camera_accel_frame:
+ camera_accel_optical_frame:
+ {}
+ camera_gyro_frame:
+ camera_gyro_optical_frame:
+ {}
ana/velodyne_base:
ana/velodyne:
{}
ana/imu_bno055_base:
ana/imu_bno055:
{}
- world:
- {}
+ ana/base_footprint_robot:
+ {}
Update Interval: 0
Value: true
- Class: rviz/Group
@@ -399,7 +422,7 @@ Visualization Manager:
Position Transformer: XYZ
Queue Size: 10
Selectable: true
- Size (Pixels): 3
+ Size (Pixels): 5
Size (m): 0.00999999978
Style: Points
Topic: /ana/sensors/nav_cam/depth_registered/points
@@ -410,9 +433,9 @@ Visualization Manager:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
- Max Value: 2.88618803
- Min Value: -0.370943904
- Value: true
+ Max Value: 3.4000001
+ Min Value: -0.180000007
+ Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
@@ -489,7 +512,7 @@ Visualization Manager:
Topic: /ana/move_base/global_costmap/footprint
Unreliable: false
Value: true
- - Alpha: 0.699999988
+ - Alpha: 0.5
Class: rviz/Map
Color Scheme: costmap
Draw Behind: true
@@ -499,7 +522,7 @@ Visualization Manager:
Unreliable: false
Use Timestamp: false
Value: true
- - Alpha: 0.699999988
+ - Alpha: 0.300000012
Class: rviz/Map
Color Scheme: costmap
Draw Behind: true
@@ -572,6 +595,28 @@ Visualization Manager:
Value: true
Enabled: true
Name: Nav
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/MarkerArray
+ Enabled: false
+ Marker Topic: /ana/lpmf/markers
+ Name: lpmf
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /ana/mht/markers
+ Name: mht
+ Namespaces:
+ cov_track: true
+ id: true
+ vel: true
+ Queue Size: 100
+ Value: true
+ Enabled: true
+ Name: People
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -620,10 +665,10 @@ Visualization Manager:
Window Geometry:
Displays:
collapsed: false
- Height: 1028
+ Height: 876
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000016a000003befc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000029c000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e007200650061006c00730065006e00730065005f0063006f006c006f007201000002ca0000011c0000001600ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006500000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf000003be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a00000326fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000231000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e007200650061006c00730065006e00730065005f0063006f006c006f0072010000025f000000ef0000001600ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006500000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000034f0000032600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -632,7 +677,7 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
- Width: 1855
+ Width: 1215
X: 65
Y: 24
realsense_color: