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
48dc9245
Commit
48dc9245
authored
Sep 26, 2018
by
dla.adolfsson@gmail.com
Browse files
added backup mapping demo
parent
4ed01a15
Changes
5
Hide whitespace changes
Inline
Side-by-side
Showing
5 changed files
with
11 additions
and
14 deletions
+11
-14
graph_map/launch/3d_fuser_ncfm2018.launch
graph_map/launch/3d_fuser_ncfm2018.launch
+3
-2
graph_map/launch/visualize.launch
graph_map/launch/visualize.launch
+2
-8
graph_map/src/graph_map/map_type.cpp
graph_map/src/graph_map/map_type.cpp
+2
-2
graph_map/src/graph_map_fuser_node.cpp
graph_map/src/graph_map_fuser_node.cpp
+3
-1
ndt_calibration/src/ndt_calib.cpp
ndt_calibration/src/ndt_calib.cpp
+1
-1
No files found.
graph_map/launch/3d_fuser_ncfm2018.launch
View file @
48dc9245
...
...
@@ -22,6 +22,9 @@
<param
name=
"initPoseFromGT"
value=
"false"
/>
<param
name=
"pose_init_x"
value=
"0"
/>
<param
name=
"pose_init_y"
value=
"0"
/>
<param
name=
"pose_init_z"
value=
"0"
/>
<param
name=
"pose_init_r"
value=
"0"
/>
<param
name=
"pose_init_p"
value=
"0"
/>
<param
name=
"pose_init_t"
value=
"0"
/>
<param
name=
"save_occupancy"
value=
"true"
/>
...
...
@@ -82,8 +85,6 @@
<param
name=
"size_z_meters"
value=
"10"
/>
<!--<param name="laser_variance_z" value="0.02" /> -->
<!--range of sensor, min range can prevent self-mapping of the robot or operator of the lidar, max range can remove uncertain measurments -->
...
...
graph_map/launch/visualize.launch
View file @
48dc9245
<?xml version="1.0" encoding="utf-8"?>
<launch>
<arg
name=
"run_bag"
default=
"false"
/>
<arg
name=
"robot_prefix"
default=
"robot5"
/>
<arg
name=
"output_pointcloud_topic_name"
default=
"/fused_cloud"
/>
<arg
name=
"rviz_enabled"
default=
"true"
/>
<group
if=
"$(arg rviz_enabled)"
>
<!-- Include visualization in rviz -->
<include
file=
"$(find graph_map)/launch/visualize_graph_fuser.launch"
>
<arg
name=
"mapping"
value=
"true"
/>
Command 'rosbag' from package 'python-rosbag' (universe)
<arg
name=
"oru-lab"
value=
"true"
/>
<arg
name=
"coop-2013"
value=
"true"
/>
</include>
</group>
...
...
graph_map/src/graph_map/map_type.cpp
View file @
48dc9245
...
...
@@ -47,8 +47,8 @@ void MapType::updateMap(const Eigen::Affine3d &Tnow,pcl::PointCloud<pcl::PointXY
//octree.setInputCloud(ptr);
//octree.addPointsFromInputCloud();
}
else
cout
<<
"Mapping disabled"
<<
endl
;
/*
else
cout<<"Mapping disabled"<<endl;
*/
}
void
MapType
::
updateMap
(
const
Eigen
::
Affine3d
&
Tnow
,
pcl
::
PointCloud
<
velodyne_pointcloud
::
PointXYZIR
>
&
cloud
,
bool
simple
){
...
...
graph_map/src/graph_map_fuser_node.cpp
View file @
48dc9245
...
...
@@ -279,10 +279,12 @@ public:
if
(
useOdometry
)
{
{
if
(
!
use_tf_listener_
){
points2_sub_
=
new
message_filters
::
Subscriber
<
velodyne_msgs
::
VelodyneScan
>
(
nh_
,
points_topic
,
2
);
cout
<<
"Using synchronized odometry and velodyne messages"
<<
endl
;
odom_sub_
=
new
message_filters
::
Subscriber
<
nav_msgs
::
Odometry
>
(
nh_
,
odometry_topic
,
10
);
sync_po_
=
new
message_filters
::
Synchronizer
<
PointsOdomSync
>
(
PointsOdomSync
(
SYNC_FRAMES
),
*
points2_sub_
,
*
odom_sub_
);
sync_po_
->
registerCallback
(
boost
::
bind
(
&
GraphMapFuserNode
::
points2OdomCallback
,
this
,
_1
,
_2
));
cout
<<
"Using synchronized odometry and velodyne messages"
<<
endl
;
}
else
{
if
(
use_pointcloud
){
...
...
@@ -505,7 +507,7 @@ public:
}
void
pointcloudCallbackTf
(
const
sensor_msgs
::
PointCloud2
::
ConstPtr
&
msg_in
){
cout
<<
"callback with tf"
<<
endl
;
pcl
::
PointCloud
<
pcl
::
PointXYZ
>
cloud
;
ros
::
Time
t
=
msg_in
->
header
.
stamp
;
pcl
::
fromROSMsg
(
*
msg_in
,
cloud
);
...
...
ndt_calibration/src/ndt_calib.cpp
View file @
48dc9245
...
...
@@ -247,7 +247,7 @@ bool NDTCalibOptimize::calibrate(Eigen::Affine3d &Ts, double &sensorTimeOffset)
Eigen
::
AngleAxis
<
double
>
(
cez
,
Eigen
::
Vector3d
::
UnitZ
())
;
geometry_msgs
::
Pose
Tsensor_msg
;
tf
::
poseEigenToMsg
(
Tsens
,
Tsensor_msg
);
std
::
cout
<<
"publish: "
<<
Tsens
.
translation
().
transpose
()
<<
std
::
endl
;
//
std::cout<<"publish: "<<Tsens.translation().transpose()<<std::endl;
sensor_pub
.
publish
(
Tsensor_msg
);
...
...
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