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
aebb4059
Commit
aebb4059
authored
Mar 27, 2019
by
simalpha
Browse files
publishing pose map_in_odom instead of TF
parent
88a5e414
Changes
3
Hide whitespace changes
Inline
Side-by-side
aicp_ros/include/aicp_ros/visualizer_ros.hpp
View file @
aebb4059
...
...
@@ -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>
...
...
@@ -51,7 +52,10 @@ public:
int
param
,
std
::
string
name
,
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,6 +68,7 @@ private:
ros
::
Publisher
prior_map_pub_
;
ros
::
Publisher
aligned_map_pub_
;
ros
::
Publisher
pose_pub_
;
ros
::
Publisher
fixed_to_odom_pub_
;
// Duplicates the list in collections renderer. assumed to be 3xN colors
std
::
vector
<
double
>
colors_
;
// Path (vector of poses)
...
...
@@ -72,9 +77,16 @@ private:
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/src/app_ros.cpp
View file @
aebb4059
...
...
@@ -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_
);
...
...
aicp_ros/src/visualizer_ros.cpp
View file @
aebb4059
...
...
@@ -18,6 +18,7 @@ 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
);
fixed_to_odom_pub_
=
nh_
.
advertise
<
geometry_msgs
::
PoseWithCovarianceStamped
>
(
fixed_to_odom_prefix_
+
fixed_frame_
+
"_to_odom"
,
10
);
colors_
=
{
51
/
255.0
,
160
/
255.0
,
44
/
255.0
,
//0
166
/
255.0
,
206
/
255.0
,
227
/
255.0
,
...
...
@@ -152,7 +153,21 @@ 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
::
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 +186,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