Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Open sidebar
software
graph_map_public
Commits
0a8d56d9
Commit
0a8d56d9
authored
Sep 21, 2018
by
dla.adolfsson@gmail.com
Browse files
changed robot
parent
bbde3552
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
21 additions
and
28 deletions
+21
-28
graph_localization/launch/3d_mcl_ncfm2018.launch
graph_localization/launch/3d_mcl_ncfm2018.launch
+8
-9
graph_localization/rviz/localization_oru_lab.rviz
graph_localization/rviz/localization_oru_lab.rviz
+10
-15
graph_localization/src/ndt_mcl_localization.cpp
graph_localization/src/ndt_mcl_localization.cpp
+3
-4
No files found.
graph_localization/launch/3d_mcl_ncfm2018.launch
View file @
0a8d56d9
...
...
@@ -5,9 +5,9 @@
<arg
name=
"run_bag"
default=
"false"
/>
<!-- Absolute Path to map to be used for localization. The map needs to be built by graph_fuser package -->
<arg
name=
"map_file_path"
default=
"$(find graph_map)/maps/ndt_map.MAP"
/>
<arg
name=
"robot_prefix"
default=
"robot
4
"
/>
<arg
name=
"robot_prefix"
default=
"robot
5
"
/>
<arg
name=
"rviz_enabled"
default=
"false"
/>
<arg
name=
"use_sim_time"
value=
"true"
/>
<!-- Include visualization in rviz -->
<group
if=
"$(arg rviz_enabled)"
>
<include
file=
"$(find graph_map)/launch/visualize_graph_fuser.launch"
>
...
...
@@ -52,8 +52,7 @@
<!--param name="mcl_tf" value="mcl_pose_est" /-->
<!-- MCL parameters -->
<!-- MCL parameters -->
<param
name=
"enable_localisation"
value=
"true"
/>
<param
name=
"resolution_local_factor"
value=
"1.3"
/>
<param
name=
"particle_count"
value=
"500"
/>
...
...
@@ -63,15 +62,15 @@
<param
name=
"z_filter_height"
value=
"1.0"
/>
</node>
<node
name=
"occupancy_map_server"
pkg=
"map_server"
type=
"map_server"
args=
" $(find graph_map)/maps/occupancy_map.yaml"
output=
"screen"
/>
<node
name=
"occupancy_map_server"
pkg=
"map_server"
type=
"map_server"
args=
" $(find graph_map)/maps/occupancy_map.yaml"
output=
"screen"
/>
<!--frame_id map_velodyne-->
<group
if=
"$(arg rviz_enabled)"
>
<node
name=
"graph_map_publisher"
pkg=
"graph_map"
type=
"graph_map_publisher"
output=
"screen"
>
<param
name=
"map_rate"
value=
"1"
/>
<param
name=
"map_file"
value=
"$(arg map_file_path)"
/>
<param
name=
"map_frame"
value=
"/maps/map_
2d_
laser"
/>
<param
name=
"map_frame"
value=
"/maps/map_laser
2d
"
/>
<param
name=
"map_parent_frame_id"
value=
"/world"
/>
<param
name=
"map_topic"
value=
"/maps/map_
2d_
laser"
/>
<param
name=
"map_topic"
value=
"/maps/map_laser
2d
"
/>
</node>
</group>
...
...
@@ -80,7 +79,7 @@
<arg
name=
"file_1"
default=
"2018-05-18-09-55-16.bag"
/>
<node
pkg=
"rosbag"
type=
"play"
name=
"player"
output=
"screen"
args=
"--clock -r 1.0 -q $(arg path)$(arg file_1)"
/>
</group>
<node
pkg=
"tf"
type=
"static_transform_publisher"
name=
"velodyne_map_static_broadcaster"
args=
"0 0 0 0 0 0 world map_velodyne 100"
/>
<node
pkg=
"tf"
type=
"static_transform_publisher"
name=
"occupancy_map_static_broadcaster"
args=
"0 0 0 0 0 0 world map 100"
/>
<node
pkg=
"tf"
type=
"static_transform_publisher"
name=
"velodyne_map_static_broadcaster"
args=
"0 0 0 0 0 0 world map_velodyne 100"
/>
<node
pkg=
"tf"
type=
"static_transform_publisher"
name=
"occupancy_map_static_broadcaster"
args=
"0 0 0 0 0 0 world map 100"
/>
</launch>
graph_localization/rviz/localization_oru_lab.rviz
View file @
0a8d56d9
...
...
@@ -14,6 +14,7 @@ Panels:
- /Odometry4/Shape1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
- /Map1
Splitter Ratio: 0.5
Tree Height: 815
- Class: rviz/Selection
...
...
@@ -64,8 +65,6 @@ Visualization Manager:
All Enabled: false
map:
Value: true
map_velodyne:
Value: true
robot4/base_footprint:
Value: true
robot4/base_fork:
...
...
@@ -133,10 +132,6 @@ Visualization Manager:
Show Names: true
Tree:
world:
map:
{}
map_velodyne:
{}
robot4/odom:
robot4/base_footprint:
robot4/base_link:
...
...
@@ -337,7 +332,7 @@ Visualization Manager:
Topic: /ndt_mcl_pose
Unreliable: false
Value: true
- Alpha:
0.800000012
- Alpha:
1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 5.9265604
...
...
@@ -348,7 +343,7 @@ Visualization Manager:
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time:
1
0
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
...
...
@@ -359,7 +354,7 @@ Visualization Manager:
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels):
1
Size (Pixels):
3
Size (m): 0.0500000007
Style: Points
Topic: /cloud_localized
...
...
@@ -411,25 +406,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/XYOrbit
Distance:
18.1015873
Distance:
22.5021648
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X:
-3.50539112
Y: -2
5.8522511
Z:
5.3598078
e-06
X:
0.926601827
Y: -2
.5325017
Z:
3.01108912
e-06
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch:
-0.0452038944
Pitch:
1.06479573
Target Frame: world
Value: XYOrbit (rviz)
Yaw: 0.
78
4042
537
Yaw: 0.
49
4042
486
Saved:
- Class: rviz/Orbit
Distance: 39.1665726
...
...
graph_localization/src/ndt_mcl_localization.cpp
View file @
0a8d56d9
...
...
@@ -235,16 +235,15 @@ class localisation_node {
Initialize
(
pose_init_geom
,
ts
);
}
tf
::
StampedTransform
transform
;
double
x
,
y
,
yaw
;
//
double x, y, yaw;
tf_listener
.
waitForTransform
(
odomTF
,
baseTF
,
ts
,
ros
::
Duration
(
0.1
));
try
{
tf_listener
.
lookupTransform
(
odomTF
,
baseTF
,
/*ros::Time(0)*/
ts
,
transform
);
yaw
=
tf
::
getYaw
(
transform
.
getRotation
());
/*
yaw = tf::getYaw(transform.getRotation());
x = transform.getOrigin().x();
y
=
transform
.
getOrigin
().
y
();
y = transform.getOrigin().y();
*/
}
catch
(
tf
::
TransformException
ex
){
ROS_ERROR
(
"%s"
,
ex
.
what
());
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment