Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Gepetto
aicp_mapping
Commits
44dd6119
Unverified
Commit
44dd6119
authored
Apr 30, 2019
by
Maurice Fallon
Committed by
GitHub
Apr 30, 2019
Browse files
Merge pull request #24 from ori-drs/sn-fix-go-back-orientation
Fix orientation of poses in go back path
parents
365f3547
d3cecd94
Changes
15
Expand all
Hide whitespace changes
Inline
Side-by-side
aicp_core/include/aicp_registration/aligned_cloud.hpp
View file @
44dd6119
...
...
@@ -18,6 +18,8 @@ public:
Eigen
::
Isometry3d
prior_pose
);
~
AlignedCloud
();
void
removePitchRollCorrection
();
void
updateCloud
(
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
&
cloud
,
bool
is_reference
);
void
updateCloud
(
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
&
cloud
,
...
...
@@ -31,6 +33,7 @@ public:
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
getCloud
(){
return
cloud_
;
}
int
getNbPoints
(){
return
cloud_
->
size
();
}
Eigen
::
Isometry3d
getOdomPose
(){
return
world_to_cloud_odom_
;
}
Eigen
::
Isometry3d
getPriorPose
(){
return
world_to_cloud_prior_
;
}
Eigen
::
Isometry3d
getCorrection
(){
return
cloud_to_reference_
;
}
Eigen
::
Isometry3d
getCorrectedPose
(){
return
world_to_cloud_corrected_
;
}
...
...
@@ -53,6 +56,8 @@ private:
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
cloud_
;
// Cloud (pre-filtered and global coordinates)
Eigen
::
Isometry3d
world_to_cloud_odom_
;
// odom to base: world -> cloud (global coordinates). this is the unmodified input.
Eigen
::
Isometry3d
world_to_cloud_prior_
;
// cloud pose prior: world -> cloud (global coordinates)
Eigen
::
Isometry3d
cloud_to_reference_
;
// relative transform: cloud -> reference (global coordinates)
Eigen
::
Isometry3d
world_to_cloud_corrected_
;
// cloud pose corrected: world -> cloud (global coordinates)
...
...
aicp_core/include/aicp_utils/common.hpp
View file @
44dd6119
...
...
@@ -22,6 +22,10 @@
//convert formats in Eigen
Eigen
::
Isometry3d
fromMatrix4fToIsometry3d
(
Eigen
::
Matrix4f
matrix
);
//compute angle between two vecotrs (returns degrees)
double
angleBetweenVectors2d
(
const
Eigen
::
Vector2d
&
v1
,
const
Eigen
::
Vector2d
&
v2
);
//swapping two values.
template
<
typename
T
>
inline
bool
swap_if_gt
(
T
&
a
,
T
&
b
)
{
...
...
@@ -45,3 +49,4 @@ Eigen::VectorXf get_random_gaussian_variable(float mean, float std_deviation, in
void
quat_to_euler
(
Eigen
::
Quaterniond
q
,
double
&
roll
,
double
&
pitch
,
double
&
yaw
);
Eigen
::
Quaterniond
euler_to_quat
(
double
roll
,
double
pitch
,
double
yaw
);
aicp_core/include/aicp_utils/visualizer.hpp
View file @
44dd6119
...
...
@@ -39,6 +39,20 @@ public:
std
::
string
name
,
int64_t
utime
=
-
1
)
=
0
;
virtual
void
publishOdomPoses
(
Eigen
::
Isometry3d
pose_
,
int
param
,
std
::
string
name
,
int64_t
utime
=
-
1
)
=
0
;
virtual
void
publishPriorPoses
(
Eigen
::
Isometry3d
pose_
,
int
param
,
std
::
string
name
,
int64_t
utime
=
-
1
)
=
0
;
virtual
void
publishOdomToMapPose
(
Eigen
::
Isometry3d
pose_
,
int64_t
utime
=
-
1
)
=
0
;
// Gets
virtual
const
PathPoses
&
getPath
()
=
0
;
...
...
aicp_core/src/registration/aligned_cloud.cpp
View file @
44dd6119
#include
"aicp_registration/aligned_cloud.hpp"
#include
"aicp_utils/common.hpp"
namespace
aicp
{
...
...
@@ -9,6 +10,8 @@ AlignedCloud::AlignedCloud(int64_t utime,
utime_
=
utime
;
cloud_
=
cloud
;
world_to_cloud_odom_
=
prior_pose
;
// prior pose. This is the original odom-to-base and is not updated elsewhere
world_to_cloud_prior_
=
prior_pose
;
// prior pose
cloud_to_reference_
=
Eigen
::
Isometry3d
::
Identity
();
// correction
world_to_cloud_corrected_
=
world_to_cloud_prior_
;
// corrected pose (set equal to prior pose when correction not available yet)
...
...
@@ -21,6 +24,35 @@ AlignedCloud::~AlignedCloud()
{
}
// Take the roll and pitch from the odom and use it to replace the roll and pitch estimated by ICP
// this is to ensure gravity consistency
// TODO: the cloud_to_reference_ is currently inconsistent with these two frames.
void
AlignedCloud
::
removePitchRollCorrection
()
{
Eigen
::
Quaterniond
q_odom
=
Eigen
::
Quaterniond
(
world_to_cloud_odom_
.
rotation
());
double
r_odom
,
p_odom
,
y_odom
;
quat_to_euler
(
q_odom
,
r_odom
,
p_odom
,
y_odom
);
Eigen
::
Quaterniond
q_corr
=
Eigen
::
Quaterniond
(
world_to_cloud_corrected_
.
rotation
());
double
r_corr
,
p_corr
,
y_corr
;
quat_to_euler
(
q_corr
,
r_corr
,
p_corr
,
y_corr
);
//std::cout << r_odom*180/M_PI << " "
// << p_odom*180/M_PI << " "
// << y_odom*180/M_PI << " odom\n";
//std::cout << r_corr*180/M_PI << " "
// << p_corr*180/M_PI << " "
// << y_corr*180/M_PI << " corr\n";
Eigen
::
Isometry3d
world_to_cloud_corrected_fixed
=
Eigen
::
Isometry3d
::
Identity
();
world_to_cloud_corrected_fixed
.
translation
()
=
world_to_cloud_corrected_
.
translation
();
world_to_cloud_corrected_fixed
.
rotate
(
euler_to_quat
(
r_odom
,
p_odom
,
y_corr
)
);
world_to_cloud_corrected_
=
world_to_cloud_corrected_fixed
;
}
void
AlignedCloud
::
updateCloud
(
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
&
cloud
,
bool
is_reference
)
{
...
...
@@ -34,6 +66,9 @@ void AlignedCloud::updateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
cloud_
=
cloud
;
cloud_to_reference_
=
correction
;
world_to_cloud_corrected_
=
cloud_to_reference_
*
world_to_cloud_prior_
;
removePitchRollCorrection
();
is_reference_
=
is_reference
;
its_reference_id_
=
its_reference_id
;
}
...
...
aicp_core/src/registration/app.cpp
View file @
44dd6119
...
...
@@ -401,10 +401,31 @@ void App::operator()() {
Eigen
::
Isometry3d
relative_motion
=
vis_
->
getPath
().
back
().
inverse
()
*
aligned_clouds_graph_
->
getLastCloud
()
->
getCorrectedPose
();
double
dist
=
relative_motion
.
translation
().
norm
();
if
(
dist
>
1.0
)
if
(
dist
>
1.0
)
//(1==1)// use threshold to reduce number of nearby markers. this shouldnt be done here
{
vis_
->
publishPoses
(
aligned_clouds_graph_
->
getLastCloud
()
->
getCorrectedPose
(),
0
,
""
,
cloud
->
getUtime
());
vis_
->
publishPriorPoses
(
aligned_clouds_graph_
->
getLastCloud
()
->
getPriorPose
(),
0
,
""
,
cloud
->
getUtime
());
vis_
->
publishOdomPoses
(
aligned_clouds_graph_
->
getLastCloud
()
->
getOdomPose
(),
0
,
""
,
cloud
->
getUtime
());
std
::
cout
<<
"odom_to_map publish <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
\n
"
;
Eigen
::
Isometry3d
odom_to_base
=
aligned_clouds_graph_
->
getLastCloud
()
->
getOdomPose
();
Eigen
::
Isometry3d
map_to_base
=
aligned_clouds_graph_
->
getLastCloud
()
->
getCorrectedPose
();
Eigen
::
Isometry3d
odom_to_map
=
(
map_to_base
*
odom_to_base
.
inverse
()).
inverse
();
vis_
->
publishOdomToMapPose
(
odom_to_map
,
cloud
->
getUtime
());
Eigen
::
Quaterniond
q_corr
=
Eigen
::
Quaterniond
(
odom_to_map
.
rotation
());
double
r_corr
,
p_corr
,
y_corr
;
quat_to_euler
(
q_corr
,
r_corr
,
p_corr
,
y_corr
);
std
::
cout
<<
odom_to_map
.
translation
().
transpose
()
<<
" odom_to_map publish
\n
"
;
std
::
cout
<<
r_corr
*
180
/
M_PI
<<
" "
<<
p_corr
*
180
/
M_PI
<<
" "
<<
y_corr
*
180
/
M_PI
<<
" rpy
\n
"
;
}
// Store aligned map and VISUALIZE
...
...
aicp_core/src/utils/common.cpp
View file @
44dd6119
...
...
@@ -22,6 +22,11 @@ Eigen::Isometry3d fromMatrix4fToIsometry3d(Eigen::Matrix4f matrix){
return
isometry
;
}
double
angleBetweenVectors2d
(
const
Eigen
::
Vector2d
&
v1
,
const
Eigen
::
Vector2d
&
v2
){
return
atan2
(
v1
(
0
)
*
v2
(
1
)
-
v1
(
1
)
*
v2
(
0
),
v1
(
0
)
*
v2
(
0
)
+
v1
(
1
)
*
v2
(
1
))
*
180.0
/
M_PI
;
}
//extract integers from a string.
std
::
string
extract_ints
(
std
::
ctype_base
::
mask
category
,
std
::
string
str
,
std
::
ctype
<
char
>
const
&
facet
)
{
...
...
@@ -71,3 +76,30 @@ void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& ya
pitch
=
asin
(
2
*
(
q0
*
q2
-
q3
*
q1
));
yaw
=
atan2
(
2
*
(
q0
*
q3
+
q1
*
q2
),
1
-
2
*
(
q2
*
q2
+
q3
*
q3
));
}
Eigen
::
Quaterniond
euler_to_quat
(
double
roll
,
double
pitch
,
double
yaw
)
{
// This conversion function introduces a NaN in Eigen Rotations when:
// roll == pi , pitch,yaw =0 ... or other combinations.
// cos(pi) ~=0 but not exactly 0
if
(
((
roll
==
M_PI
)
&&
(
pitch
==
0
))
&&
(
yaw
==
0
)){
return
Eigen
::
Quaterniond
(
0
,
1
,
0
,
0
);
}
else
if
(
((
pitch
==
M_PI
)
&&
(
roll
==
0
))
&&
(
yaw
==
0
)){
return
Eigen
::
Quaterniond
(
0
,
0
,
1
,
0
);
}
else
if
(
((
yaw
==
M_PI
)
&&
(
roll
==
0
))
&&
(
pitch
==
0
)){
return
Eigen
::
Quaterniond
(
0
,
0
,
0
,
1
);
}
double
sy
=
sin
(
yaw
*
0.5
);
double
cy
=
cos
(
yaw
*
0.5
);
double
sp
=
sin
(
pitch
*
0.5
);
double
cp
=
cos
(
pitch
*
0.5
);
double
sr
=
sin
(
roll
*
0.5
);
double
cr
=
cos
(
roll
*
0.5
);
double
w
=
cr
*
cp
*
cy
+
sr
*
sp
*
sy
;
double
x
=
sr
*
cp
*
cy
-
cr
*
sp
*
sy
;
double
y
=
cr
*
sp
*
cy
+
sr
*
cp
*
sy
;
double
z
=
cr
*
cp
*
sy
-
sr
*
sp
*
cy
;
return
Eigen
::
Quaterniond
(
w
,
x
,
y
,
z
);
}
aicp_ros/config/rviz/config_aicp.rviz
View file @
44dd6119
This diff is collapsed.
Click to expand it.
aicp_ros/include/aicp_ros/visualizer_ros.hpp
View file @
44dd6119
...
...
@@ -5,6 +5,7 @@
#include
"aicp_utils/visualizer.hpp"
#include
<geometry_msgs/PoseStamped.h>
#include
<geometry_msgs/PoseWithCovarianceStamped.h>
#include
<nav_msgs/Path.h>
#include
<eigen_conversions/eigen_msg.h>
...
...
@@ -50,8 +51,24 @@ public:
void
publishPoses
(
PathPoses
poses
,
int
param
,
std
::
string
name
,
int64_t
utime
);
void
publishOdomPoses
(
Eigen
::
Isometry3d
pose
,
int
param
,
std
::
string
name
,
int64_t
utime
);
void
publishOdomPoses
(
PathPoses
poses
,
int
param
,
std
::
string
name
,
int64_t
utime
);
void
publishPriorPoses
(
Eigen
::
Isometry3d
pose
,
int
param
,
std
::
string
name
,
int64_t
utime
);
void
publishPriorPoses
(
PathPoses
poses
,
int
param
,
std
::
string
name
,
int64_t
utime
);
void
publishOdomToMapPose
(
Eigen
::
Isometry3d
pose
,
int64_t
utime
);
// Publish tf from fixed_frame to odom
void
publishFixedFrameToOdomTF
(
Eigen
::
Isometry3d
&
fixed_frame_to_base_eigen
,
ros
::
Time
msg_time
);
void
publishFixedFrameToOdomTF
(
const
Eigen
::
Isometry3d
&
fixed_frame_to_base_eigen
,
ros
::
Time
msg_time
);
void
publishFixedFrameToOdomPose
(
const
Eigen
::
Isometry3d
&
fixed_frame_to_base_eigen
,
ros
::
Time
msg_time
);
// Gets
const
PathPoses
&
getPath
(){
...
...
@@ -64,17 +81,32 @@ private:
ros
::
Publisher
prior_map_pub_
;
ros
::
Publisher
aligned_map_pub_
;
ros
::
Publisher
pose_pub_
;
ros
::
Publisher
odom_pose_pub_
;
ros
::
Publisher
prior_pose_pub_
;
ros
::
Publisher
fixed_to_odom_pub_
;
ros
::
Publisher
odom_to_map_pub_
;
// Duplicates the list in collections renderer. assumed to be 3xN colors
std
::
vector
<
double
>
colors_
;
// Path (vector of poses)
PathPoses
path_
;
PathPoses
odom_path_
;
PathPoses
prior_path_
;
std
::
string
fixed_frame_
;
// map or map_test
std
::
string
odom_frame_
;
std
::
string
base_frame_
;
std
::
string
fixed_to_odom_prefix_
=
"/localization_manager/"
;
// TF listener and broadcaster
tf
::
TransformListener
tf_listener_
;
tf
::
TransformBroadcaster
tf_broadcaster_
;
geometry_msgs
::
PoseWithCovarianceStamped
fixed_to_odom_msg_
;
tf
::
Pose
temp_tf_pose_
;
void
computeFixedFrameToOdom
(
const
Eigen
::
Isometry3d
&
fixed_frame_to_base_eigen
,
Eigen
::
Isometry3d
&
fixed_frame_to_odom_eigen
);
};
}
aicp_ros/launch/aicp.launch
View file @
44dd6119
...
...
@@ -31,6 +31,10 @@
<param
name=
"working_mode"
value=
"debug"
/>
<!-- Fixed frame (default: map, debug: map_test -->
<param
name=
"fixed_frame"
value=
"$(arg fixed_frame)"
/>
<!-- default is /aicp/pose_corrected -->
<param
name=
"output_channel"
value=
"/aicp/pose_corrected"
/>
<param
name=
"lidar_channel"
value=
"/point_cloud_filter/velodyne/point_cloud_filtered"
/>
<!-- Prior map info -->
<param
name=
"load_map_from_file"
value=
"$(arg load_map_from_file)"
/>
<param
name=
"map_from_file_path"
value=
"$(arg map_from_file_path)"
/>
...
...
aicp_ros/launch/view_recorded_rosbag_aicp_simple.launch
0 → 100644
View file @
44dd6119
<?xml version="1.0" encoding="utf-8"?>
<launch>
<arg
name=
"rviz"
default=
"true"
/>
<arg
name=
"rviz_minimal"
default=
"true"
/>
<arg
name=
"quadruped_name"
default=
"anymal"
/>
<arg
name=
"quadruped_setup"
default=
"minimal_mb_inspection_v_1_2"
/>
<arg
name=
"description_name"
default=
"quadruped_description"
/>
<arg
name=
"world"
default=
"empty"
/>
<param
name=
"use_sim_time"
value=
"true"
/>
<!-- Load robot description -->
<include
file=
"$(find anymal_boxy_description)/launch/load.launch"
>
<arg
name=
"description_name"
value=
"$(arg description_name)"
/>
<arg
name=
"simulation"
value=
"false"
/>
<arg
name=
"velodyne"
value=
"true"
/>
<arg
name=
"realsense"
value=
"true"
/>
</include>
<arg
name=
"rviz_config_path"
default=
"$(find aicp_ros)/config/rviz/config_aicp.rviz"
/>
<!-- rviz -->
<group
if=
"$(arg rviz)"
>
<include
file=
"$(find anymal_rviz_visualization)/launch/rviz.launch"
>
<arg
name=
"rviz_world_path"
value=
"$(arg world)"
/>
<arg
name=
"rviz_config_path"
value=
"$(arg rviz_config_path)"
/>
<arg
name=
"tf_minimal"
value=
"false"
/>
</include>
</group>
</launch>
aicp_ros/python/aicp_tf_bridge.py
0 → 100755
View file @
44dd6119
#!/usr/bin/env python
# pass aicp's correction to tf
# this can be used instead of localization manager.
import
rospy
from
geometry_msgs.msg
import
TransformStamped
from
geometry_msgs.msg
import
*
from
tf2_msgs.msg
import
TFMessage
from
visualization_msgs.msg
import
Marker
import
tf
import
math
import
numpy
import
time
pub
=
rospy
.
Publisher
(
"/tf"
,
TFMessage
,
queue_size
=
10
)
vis_pub
=
rospy
.
Publisher
(
"/visualization_marker"
,
Marker
,
queue_size
=
10
)
rospy
.
loginfo
(
"aicp_tf_bridge"
)
def
handle_odom_to_map
(
msg
):
print
"x"
#
tfm
=
TFMessage
()
this_t
=
TransformStamped
()
this_t
.
header
=
msg
.
header
this_t
.
header
.
frame_id
=
"odom"
this_t
.
child_frame_id
=
"map"
this_t
.
transform
.
translation
=
msg
.
pose
.
pose
.
position
this_t
.
transform
.
rotation
=
msg
.
pose
.
pose
.
orientation
tfm
.
transforms
.
append
(
this_t
)
pub
.
publish
(
tfm
)
rospy
.
init_node
(
'aicp_tf_bridge'
)
rospy
.
Subscriber
(
'/icp_tools/map_pose'
,
PoseWithCovarianceStamped
,
handle_odom_to_map
)
rospy
.
spin
()
aicp_ros/python/aicp_tf_filter.py
0 → 100644
View file @
44dd6119
#!/usr/bin/env python
# when playing back a bag, filter out odom-to-map from tf
# Note: need to replay the bag while remapping /tf to /tf_old
# rosbag play filename.bag --pause --clock /tf:=/tf_old
import
rospy
from
tf2_msgs.msg
import
TFMessage
import
tf
import
math
import
numpy
import
time
tf_new_pub
=
rospy
.
Publisher
(
"/tf"
,
TFMessage
,
queue_size
=
10
)
def
handle_odom_to_map
(
msg
):
for
t
in
msg
.
transforms
:
if
(
t
.
child_frame_id
==
"map"
):
print
"y"
return
tf_new_pub
.
publish
(
msg
)
rospy
.
init_node
(
'aicp_tf_filter'
)
rospy
.
Subscriber
(
'/tf_old'
,
TFMessage
,
handle_odom_to_map
)
rospy
.
spin
()
aicp_ros/src/app_ros.cpp
View file @
44dd6119
...
...
@@ -90,7 +90,8 @@ void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedCon
// Publish fixed_frame to odom tf
ros
::
Time
msg_time
(
pose_msg_in
->
header
.
stamp
.
sec
,
pose_msg_in
->
header
.
stamp
.
nsec
);
vis_ros_
->
publishFixedFrameToOdomTF
(
corrected_pose_
,
msg_time
);
// vis_ros_->publishFixedFrameToOdomTF(corrected_pose_, msg_time);
// vis_ros_->publishFixedFrameToOdomPose(corrected_pose_, msg_time);
// Publish /aicp/pose_corrected
tf
::
poseEigenToTF
(
corrected_pose_
,
temp_tf_pose_
);
...
...
@@ -179,7 +180,7 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
// vis_->publishCloud(accumulated_cloud, 10, "/aicp/accumulated_cloud", accu_->getFinishedTime());
// Push this cloud onto the work queue (mutex safe)
const
int
max_queue_size
=
100
;
const
int
max_queue_size
=
3
;
{
std
::
unique_lock
<
std
::
mutex
>
lock
(
data_mutex_
);
...
...
@@ -193,6 +194,8 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
cloud_queue_
.
push_back
(
current_cloud
);
if
(
cloud_queue_
.
size
()
>
max_queue_size
)
{
cout
<<
"|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
\n
"
;
cout
<<
"|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
\n
"
;
cout
<<
"[App ROS] WARNING: dropping "
<<
(
cloud_queue_
.
size
()
-
max_queue_size
)
<<
" clouds."
<<
endl
;
}
...
...
aicp_ros/src/talker_ros.cpp
View file @
44dd6119
#include
"aicp_ros/talker_ros.hpp"
//#include <eigen_conversions/eigen_msg.h>
#include
"aicp_utils/common.hpp"
namespace
aicp
{
...
...
@@ -46,11 +45,27 @@ void ROSTalker::reversePath(PathPoses& path){
std
::
reverse
(
path
.
begin
(),
path
.
end
());
for
(
size_t
i
=
0
;
i
<
path
.
size
();
++
i
){
// Turn 90 degrees about z-axis
double
angle
=
180.0
;
// Compute orientation along trajectory line
if
(
i
!=
path
.
size
()
-
1
)
{
// angle between x-axis of current frame
Eigen
::
Vector2d
v1
;
v1
=
path
[
i
].
matrix
().
block
<
2
,
1
>
(
0
,
0
);
// and trajectory line
Eigen
::
Vector2d
v2
;
v2
<<
path
[
i
+
1
].
translation
()(
0
,
0
)
-
path
[
i
].
translation
()(
0
,
0
),
path
[
i
+
1
].
translation
()(
1
,
0
)
-
path
[
i
].
translation
()(
1
,
0
);
angle
=
angleBetweenVectors2d
(
v1
,
v2
);
}
// Turn about z-axis
Eigen
::
Matrix3d
rot
;
rot
=
Eigen
::
AngleAxisd
(
0
*
M_PI
/
180
,
Eigen
::
Vector3d
::
UnitX
())
*
Eigen
::
AngleAxisd
(
0
*
M_PI
/
180
,
Eigen
::
Vector3d
::
UnitY
())
*
Eigen
::
AngleAxisd
(
180
*
M_PI
/
180
,
Eigen
::
Vector3d
::
UnitZ
());
*
Eigen
::
AngleAxisd
(
angle
*
M_PI
/
180
,
Eigen
::
Vector3d
::
UnitZ
());
path
[
i
].
linear
()
=
rot
*
path
[
i
].
rotation
();
}
...
...
aicp_ros/src/visualizer_ros.cpp
View file @
44dd6119
...
...
@@ -18,6 +18,13 @@ ROSVisualizer::ROSVisualizer(ros::NodeHandle& nh, string fixed_frame) : nh_(nh),
prior_map_pub_
=
nh_
.
advertise
<
sensor_msgs
::
PointCloud2
>
(
"/aicp/prior_map"
,
10
);
aligned_map_pub_
=
nh_
.
advertise
<
sensor_msgs
::
PointCloud2
>
(
"/aicp/aligned_map"
,
10
);
pose_pub_
=
nh_
.
advertise
<
nav_msgs
::
Path
>
(
"/aicp/poses"
,
100
);
odom_pose_pub_
=
nh_
.
advertise
<
nav_msgs
::
Path
>
(
"/aicp/odom_poses"
,
100
);
prior_pose_pub_
=
nh_
.
advertise
<
nav_msgs
::
Path
>
(
"/aicp/prior_poses"
,
100
);
fixed_to_odom_pub_
=
nh_
.
advertise
<
geometry_msgs
::
PoseWithCovarianceStamped
>
(
fixed_to_odom_prefix_
+
fixed_frame_
+
"_to_odom"
,
10
);
odom_to_map_pub_
=
nh_
.
advertise
<
geometry_msgs
::
PoseWithCovarianceStamped
>
(
"/icp_tools/map_pose"
,
10
);
colors_
=
{
51
/
255.0
,
160
/
255.0
,
44
/
255.0
,
//0
166
/
255.0
,
206
/
255.0
,
227
/
255.0
,
...
...
@@ -152,7 +159,87 @@ void ROSVisualizer::publishPoses(PathPoses poses, int param, string name, int64_
pose_pub_
.
publish
(
path_msg
);
}
void
ROSVisualizer
::
publishFixedFrameToOdomTF
(
Eigen
::
Isometry3d
&
fixed_frame_to_base_eigen
,
ros
::
Time
msg_time
)
void
ROSVisualizer
::
publishOdomPoses
(
Eigen
::
Isometry3d
pose
,
int
param
,
std
::
string
name
,
int64_t
utime
)
{
odom_path_
.
push_back
(
pose
);
publishOdomPoses
(
odom_path_
,
param
,
name
,
utime
);
}
void
ROSVisualizer
::
publishOdomPoses
(
PathPoses
poses
,
int
param
,
string
name
,
int64_t
utime
){
nav_msgs
::
Path
path_msg
;
int
secs
=
utime
*
1E-6
;
int
nsecs
=
(
utime
-
(
secs
*
1E6
))
*
1E3
;
path_msg
.
header
.
stamp
=
ros
::
Time
(
secs
,
nsecs
);
path_msg
.
header
.
frame_id
=
fixed_frame_
;
for
(
size_t
i
=
0
;
i
<
poses
.
size
();
++
i
){
geometry_msgs
::
PoseStamped
m
;
m
.
header
.
stamp
=
ros
::
Time
(
secs
,
nsecs
);
m
.
header
.
frame_id
=
fixed_frame_
;
tf
::
poseEigenToMsg
(
poses
[
i
],
m
.
pose
);
path_msg
.
poses
.
push_back
(
m
);
}
odom_pose_pub_
.
publish
(
path_msg
);
}
void
ROSVisualizer
::
publishPriorPoses
(
Eigen
::
Isometry3d
pose
,
int
param
,
std
::
string
name
,
int64_t
utime
)
{
prior_path_
.
push_back
(
pose
);
publishPriorPoses
(
odom_path_
,
param
,
name
,
utime
);
}
void
ROSVisualizer
::
publishPriorPoses
(
PathPoses
poses
,
int
param
,
string
name
,
int64_t
utime
){
nav_msgs
::
Path
path_msg
;
int
secs
=
utime
*
1E-6
;
int
nsecs
=
(
utime
-
(
secs
*
1E6
))
*
1E3
;
path_msg
.
header
.
stamp
=
ros
::
Time
(
secs
,
nsecs
);
path_msg
.
header
.
frame_id
=
fixed_frame_
;
for
(
size_t
i
=
0
;
i
<
poses
.
size
();
++
i
){
geometry_msgs
::
PoseStamped
m
;
m
.
header
.
stamp
=
ros
::
Time
(
secs
,
nsecs
);
m
.
header
.
frame_id
=
fixed_frame_
;
tf
::
poseEigenToMsg
(
poses
[
i
],
m
.
pose
);
path_msg
.
poses
.
push_back
(
m
);
}
prior_pose_pub_
.
publish
(
path_msg
);
}
void
ROSVisualizer
::
publishOdomToMapPose
(
Eigen
::
Isometry3d
pose
,
int64_t
utime
)
{
int
secs
=
utime
*
1E-6
;
int
nsecs
=
(
utime
-
(
secs
*
1E6
))
*
1E3
;
geometry_msgs
::
PoseWithCovarianceStamped
msg
;
msg
.
header
.
stamp
=
ros
::
Time
(
secs
,
nsecs
);
msg
.
header
.
frame_id
=
"odom"
;
tf
::
poseEigenToMsg
(
pose
,
msg
.
pose
.
pose
);
odom_to_map_pub_
.
publish
(
msg
);
}
void
ROSVisualizer
::
publishFixedFrameToOdomPose
(
const
Eigen
::
Isometry3d
&
fixed_frame_to_base_eigen
,
ros
::
Time
msg_time
)
{
Eigen
::
Isometry3d
fixed_frame_to_odom_eigen
;
this
->
computeFixedFrameToOdom
(
fixed_frame_to_base_eigen
,
fixed_frame_to_odom_eigen
);
tf
::
poseEigenToTF
(
fixed_frame_to_odom_eigen
,
temp_tf_pose_
);
tf
::
poseTFToMsg
(
temp_tf_pose_
,
fixed_to_odom_msg_
.
pose
.
pose
);
fixed_to_odom_msg_
.
header
.
stamp
=
msg_time
;
fixed_to_odom_msg_
.
header
.
frame_id
=
odom_frame_
;
fixed_to_odom_pub_
.
publish
(
fixed_to_odom_msg_
);
}
void
ROSVisualizer
::
computeFixedFrameToOdom
(
const
Eigen
::
Isometry3d
&
fixed_frame_to_base_eigen
,
Eigen
::
Isometry3d
&
fixed_frame_to_odom_eigen
)
{
// TF listener
tf
::
StampedTransform
base_to_odom_tf
;
...
...
@@ -171,8 +258,14 @@ void ROSVisualizer::publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_
tf
::
transformTFToEigen
(
base_to_odom_tf
,
base_to_odom_eigen
);
// Multiply
Eigen
::
Isometry3d
fixed_frame_to_odom_eigen
;
fixed_frame_to_odom_eigen
=
fixed_frame_to_base_eigen
*
base_to_odom_eigen
.
inverse
();
}
void
ROSVisualizer
::
publishFixedFrameToOdomTF
(
const
Eigen
::
Isometry3d
&
fixed_frame_to_base_eigen
,
ros
::
Time
msg_time
)
{
Eigen
::
Isometry3d
fixed_frame_to_odom_eigen
;
this
->
computeFixedFrameToOdom
(
fixed_frame_to_base_eigen
,
fixed_frame_to_odom_eigen
);
// Convert to TF
tf
::
StampedTransform
fixed_frame_to_odom_tf
;
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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