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
c3e159a0
Commit
c3e159a0
authored
Sep 22, 2018
by
dla.adolfsson@gmail.com
Browse files
fixing initialization
parent
0a8d56d9
Changes
7
Hide whitespace changes
Inline
Side-by-side
Showing
7 changed files
with
46 additions
and
64 deletions
+46
-64
graph_localization/launch/3d_mcl_ncfm2018.launch
graph_localization/launch/3d_mcl_ncfm2018.launch
+6
-4
graph_localization/rviz/localization_oru_lab.rviz
graph_localization/rviz/localization_oru_lab.rviz
+16
-46
graph_localization/src/ndt_mcl_localization.cpp
graph_localization/src/ndt_mcl_localization.cpp
+1
-2
graph_map/include/graph_map/visualization/graph_visualization.h
...map/include/graph_map/visualization/graph_visualization.h
+3
-0
graph_map/src/graph_map_fuser_node.cpp
graph_map/src/graph_map_fuser_node.cpp
+15
-9
graph_map/src/graph_map_publisher.cpp
graph_map/src/graph_map_publisher.cpp
+2
-0
graph_map/src/visualization/graph_visualization.cpp
graph_map/src/visualization/graph_visualization.cpp
+3
-3
No files found.
graph_localization/launch/3d_mcl_ncfm2018.launch
View file @
c3e159a0
...
...
@@ -7,7 +7,8 @@
<arg
name=
"map_file_path"
default=
"$(find graph_map)/maps/ndt_map.MAP"
/>
<arg
name=
"robot_prefix"
default=
"robot5"
/>
<arg
name=
"rviz_enabled"
default=
"false"
/>
<arg
name=
"use_sim_time"
value=
"true"
/>
<arg
name=
"root_id"
default=
"/world"
/>
<!-- Include visualization in rviz -->
<group
if=
"$(arg rviz_enabled)"
>
<include
file=
"$(find graph_map)/launch/visualize_graph_fuser.launch"
>
...
...
@@ -45,7 +46,7 @@
<param
name=
"pose_init_t"
value=
"0"
/>
<!-- world frame id -->
<param
name=
"root_tf"
value=
"
world
"
/>
<param
name=
"root_tf"
value=
"
$(arg root_id)
"
/>
<!-- odometry frame id-->
<param
name=
"odom_tf"
value=
"$(arg robot_prefix)/odom"
/>
<param
name=
"base_tf"
value=
"$(arg robot_prefix)/base_footprint"
/>
...
...
@@ -64,12 +65,13 @@
<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_laser2d"
/>
<param
name=
"map_parent_frame_id"
value=
"
/world
"
/>
<param
name=
"map_parent_frame_id"
value=
"
$(arg root_id)
"
/>
<param
name=
"map_topic"
value=
"/maps/map_laser2d"
/>
</node>
</group>
...
...
@@ -79,7 +81,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=
"velodyne_map_static_broadcaster"
args=
"0 0 0 0 0 0 world map_
laser2d
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 @
c3e159a0
...
...
@@ -11,9 +11,11 @@ Panels:
- /Odometry1/Shape1
- /Odometry2/Shape1
- /Odometry3/Shape1
- /PoseArray1
- /Odometry4/Shape1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
- /NDT1
- /Map1
Splitter Ratio: 0.5
Tree Height: 815
...
...
@@ -36,7 +38,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource:
PointCloud2
SyncSource:
""
Visualization Manager:
Class: ""
Displays:
...
...
@@ -65,6 +67,8 @@ Visualization Manager:
All Enabled: false
map:
Value: true
map_laser2d:
Value: true
robot4/base_footprint:
Value: true
robot4/base_fork:
...
...
@@ -121,8 +125,6 @@ Visualization Manager:
Value: true
submap_node0:
Value: true
velodyne:
Value: true
world:
Value: true
Marker Scale: 1
...
...
@@ -132,42 +134,10 @@ Visualization Manager:
Show Names: true
Tree:
world:
robot4/odom:
robot4/base_footprint:
robot4/base_link:
robot4/base_fork:
robot4/left_fork:
{}
robot4/right_fork:
{}
robot4/fixed_left_wheel_link:
{}
robot4/fixed_right_wheel_link:
{}
robot4/kinect2_link:
robot4/kinect2_depth_optical_frame:
{}
robot4/kinect2_rgb_optical_frame:
robot4/kinect2_ir_optical_frame:
{}
robot4/kinect_link:
{}
robot4/laser2d_floor:
{}
robot4/laser2d_floor_link:
{}
robot4/laser2d_top:
{}
robot4/laser2d_top_link:
{}
robot4/robot4/velodyne_base_link:
robot4/robot4/velodyne:
{}
robot4/steer_link:
robot4/sd_wheel_link:
{}
velodyne:
{}
map:
{}
map_laser2d:
{}
submap_node0:
{}
Update Interval: 0
...
...
@@ -406,25 +376,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/XYOrbit
Distance:
22.5021648
Distance:
66.5617294
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X:
0.926601827
Y: -
2.5325017
Z:
3.01108912
e-0
6
X:
7.46893883
Y: -
6.24155045
Z:
1.15941584
e-0
5
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.
06479573
Pitch: 1.
25979626
Target Frame: world
Value: XYOrbit (rviz)
Yaw:
0.494042486
Yaw:
6.12041521
Saved:
- Class: rviz/Orbit
Distance: 39.1665726
...
...
@@ -452,7 +422,7 @@ Window Geometry:
Height: 1028
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000
1aa
000003befc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003be000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003befc0200000003fb0000000a005600690065007700730100000028000003be000000ad00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000064f000000b0fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006500000000000000073f0000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000
47a
000003be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd000000040000000000000
2a7
000003befc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003be000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003befc0200000003fb0000000a005600690065007700730100000028000003be000000ad00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000064f000000b0fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006500000000000000073f0000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000
37d
000003be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
...
...
graph_localization/src/ndt_mcl_localization.cpp
View file @
c3e159a0
...
...
@@ -167,9 +167,8 @@ class localisation_node {
tf
::
poseMsgToEigen
(
pose_init
,
pose_init_eig
);
cout
<<
"Initial position set to"
<<
pose_init_eig
.
translation
().
transpose
()
<<
endl
;
Vector6d
var
;
var
<<
0.5
,
0.5
,
0.0
,
0.0
,
0.0
,
0.
2
;
var
<<
1.0
,
1.0
,
0.0
,
0.0
,
0.0
,
0.
5
;
localisation_type_ptr_
->
InitializeLocalization
(
pose_init_eig
,
var
);
sleep
(
1
);
initialized
=
true
;
}
void
InitializeUniform
(
const
geometry_msgs
::
Pose
&
pose_init
,
const
ros
::
Time
&
t_init
){
...
...
graph_map/include/graph_map/visualization/graph_visualization.h
View file @
c3e159a0
...
...
@@ -44,6 +44,8 @@ public:
void
PlotActiveNodesCloud
();
void
SetParentFrameId
(
const
std
::
string
&
parent_frame_id
){
parent_frame_id_
=
parent_frame_id
;}
void
PlotAllClouds
();
void
PlotLinks
(
ros
::
Time
t
=
ros
::
Time
::
now
());
...
...
@@ -72,6 +74,7 @@ void PlotGraphThread();
std
::
string
tf_prefix_
=
"/tf"
;
ros
::
NodeHandle
nh_
;
GraphMapNavigatorPtr
graph_map_
;
std
::
string
parent_frame_id_
=
"/world"
;
bool
visualization_enabled_
=
false
;
bool
visualize_map_
=
false
;
...
...
graph_map/src/graph_map_fuser_node.cpp
View file @
c3e159a0
...
...
@@ -356,12 +356,12 @@ public:
pcl
::
PointCloud
<
pcl
::
PointXYZ
>
registered_cloud
=
cloud
;
ros
::
Time
ts
;
pcl_conversions
::
fromPCL
(
cloud
.
header
.
stamp
,
ts
);
bool
uppdated
=
fuser_
->
ProcessFrame
<
pcl
::
PointXYZ
>
(
cloud
,
pose_
,
Tmotion
);
pcl_conversions
::
fromPCL
(
cloud
.
header
.
stamp
,
ts
);
bool
uppdated
=
fuser_
->
ProcessFrame
<
pcl
::
PointXYZ
>
(
cloud
,
pose_
,
Tmotion
);
tf
::
Transform
Transform
;
tf
::
transformEigenToTF
(
pose_
,
Transform
);
tf_
.
sendTransform
(
tf
::
StampedTransform
(
Transform
,
ts
,
world_link_id
,
fuser_base_link_id
));
tf_
.
sendTransform
(
tf
::
StampedTransform
(
tf_sensor_pose_
,
ts
,
fuser_base_link_id
,
laser_link_id
));
//
tf_.sendTransform(tf::StampedTransform(Transform, ts, world_link_id, fuser_base_link_id));
//
tf_.sendTransform(tf::StampedTransform(tf_sensor_pose_, ts, fuser_base_link_id, laser_link_id));
if
(
use_tf_listener_
){
tf
::
Transform
tf_pose
;
tf
::
poseEigenToTF
(
pose_
,
tf_pose
);
...
...
@@ -421,14 +421,16 @@ public:
return
false
;
}
bool
GetTransformFromTF
(
const
ros
::
Time
&
time
,
tf
::
StampedTransform
&
transform
)
{
bool
GetTransformFromTF
(
const
ros
::
Time
&
time
,
tf
::
StampedTransform
&
transform
)
{
static
tf
::
TransformListener
tf_listener
;
tf_listener
.
waitForTransform
(
odometry_link_id
,
base_footprint_id
,
time
,
ros
::
Duration
(
0.1
));
try
{
tf_listener
.
lookupTransform
(
odometry_link_id
,
base_footprint_id
,
time
,
transform
);
}
catch
(
tf
::
TransformException
ex
){
ROS_ERROR
(
"%s"
,
ex
.
what
());
//ROS_ERROR("%s", ex.what());
cerr
<<
"No transformation between "
<<
odometry_link_id
<<
", and "
<<
base_footprint_id
<<
" at time"
<<
time
.
toSec
()
<<
endl
;
return
false
;
}
return
true
;
...
...
@@ -504,13 +506,17 @@ public:
void
points2OdomCallbackTF
(
const
velodyne_msgs
::
VelodyneScan
::
ConstPtr
&
msg_in
){
//this callback is used to look up tf transformation for scan data
pcl
::
PointCloud
<
pcl
::
PointXYZ
>
cloud
;
ros
::
Time
t_cloud_end
=
msg_in
->
packets
[
msg_in
->
packets
.
size
()
-
1
].
stamp
;
ros
::
Time
t_cloud_end
=
msg_in
->
header
.
stamp
;
//msg_in->packets[msg_in->packets.size()-1].stamp;
cerr
<<
"Time: "
<<
t_cloud_end
<<
endl
;
ndt_generic
::
UnwarpCloudSimple
<
pcl
::
PointXYZ
>
(
velodyne_parser
,
msg_in
,
cloud
);
Eigen
::
Affine3d
Tm
,
Todo_eig
;
GetTransformFromTF
(
t_cloud_end
,
T_odom
);
if
(
!
GetTransformFromTF
(
t_cloud_end
,
T_odom
))
return
;
tf
::
poseTFToEigen
(
T_odom
,
Todo_eig
);
cerr
<<
"FOUND TRANSFORMATION between odometry_link_id= "
<<
odometry_link_id
<<
", base_footprint_id="
<<
base_footprint_id
<<
" , trans="
<<
Todo_eig
.
translation
().
transpose
()
<<
" at time t="
<<
time
<<
endl
;
Tm
=
GetOdomdiff
(
Todo_eig
);
cout
<<
"use listener, Tm="
<<
Tm
.
translation
().
transpose
()
<<
endl
;
cout
<<
"Tm="
<<
Tm
.
translation
().
transpose
()
<<
endl
;
pcl_conversions
::
toPCL
(
t_cloud_end
,
cloud
.
header
.
stamp
);
this
->
processFrame
(
cloud
,
Tm
);
}
...
...
graph_map/src/graph_map_publisher.cpp
View file @
c3e159a0
...
...
@@ -47,6 +47,8 @@ int main(int argc, char** argv){
LoadGraphMap
(
mapFile
,
graph_map_
);
graph_map
::
MapTypePtr
map_type
=
graph_map_
->
GetCurrentNode
()
->
GetMap
();
graphVisualization
vis
(
graph_map_
,
true
);
vis
.
SetParentFrameId
(
mapParentFrameId
);
NDTMapPtr
ptr
=
boost
::
dynamic_pointer_cast
<
NDTMapType
>
(
map_type
);
perception_oru
::
NDTMap
*
ndt_ptr
=
ptr
->
GetNDTMap
();
tf
::
TransformBroadcaster
br
;
...
...
graph_map/src/visualization/graph_visualization.cpp
View file @
c3e159a0
...
...
@@ -92,7 +92,7 @@ void graphVisualization::PlotOccupancyMap(const ros::Time t){
void
graphVisualization
::
PlotLinks
(
ros
::
Time
t
){
std
::
string
prev_frame_id
=
"world"
;
std
::
string
prev_frame_id
=
parent_frame_id_
;
for
(
mapNodeItr
itr
=
graph_map_
->
begin
();
itr
!=
graph_map_
->
end
();
itr
++
){
tf
::
Transform
node_tf
;
Eigen
::
Affine3d
diff
;
...
...
@@ -141,7 +141,7 @@ void graphVisualization::PlotCurrentLinkInfo(ros::Time t){
void
graphVisualization
::
PlotAllClouds
(){
pcl
::
PointCloud
<
pcl
::
PointXYZ
>
cloud
;
graph_map_
->
GetCloud
(
cloud
,
true
);
cloud
.
header
.
frame_id
=
"/world"
;
cloud
.
header
.
frame_id
=
parent_frame_id_
;
pcl_conversions
::
toPCL
(
ros
::
Time
::
now
(),
cloud
.
header
.
stamp
);
cloud_world_pub
.
publish
(
cloud
);
}
...
...
@@ -167,7 +167,7 @@ void graphVisualization::PlotTrajectory(ros::Time t){
geometry_msgs
::
PoseArray
pose_arr
;
geometry_msgs
::
Pose
pose
;
pose_arr
.
header
.
frame_id
=
"/world"
;
pose_arr
.
header
.
frame_id
=
parent_frame_id_
;
pose_arr
.
header
.
stamp
=
t
;
for
(
mapNodeItr
itr
=
graph_map_
->
begin
();
itr
!=
graph_map_
->
end
();
itr
++
){
ndt_generic
::
Affine3dSTLVek
vek
=
(
*
itr
)
->
GetMap
()
->
GetObservationVector
();
...
...
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