-
Notifications
You must be signed in to change notification settings - Fork 39
/
rviz_path.cpp
54 lines (44 loc) · 1.29 KB
/
rviz_path.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
#include <include/rviz_path.h>
RVIZPath::RVIZPath(ros::NodeHandle& nh, string topic_name, string frame_id, int bufferCount, int maxNumOfPose)
{
path_pub = nh.advertise<nav_msgs::Path>(topic_name, bufferCount);
if(maxNumOfPose==-1)
{
numOfPose = DEFAULT_NUM_OF_POSE;
}else
{
numOfPose = maxNumOfPose;
}
this->frame_id_path = frame_id;
path.header.frame_id = frame_id_path;
}
void RVIZPath::pubPathT_c_w(const SE3 T_c_w, const ros::Time stamp)
{
pubPathT_w_c(T_c_w.inverse(),stamp);
}
void RVIZPath::pubPathT_w_c(const SE3 T_w_c, const ros::Time stamp)
{
geometry_msgs::PoseStamped poseStamped;
poseStamped.header.frame_id =frame_id_path;
poseStamped.header.stamp = stamp;
Quaterniond q = T_w_c.so3().unit_quaternion();
Vec3 t = T_w_c.translation();
poseStamped.pose.orientation.w = q.w();
poseStamped.pose.orientation.x = q.x();
poseStamped.pose.orientation.y = q.y();
poseStamped.pose.orientation.z = q.z();
poseStamped.pose.position.x = t[0];
poseStamped.pose.position.y = t[1];
poseStamped.pose.position.z = t[2];
path.header.stamp = stamp;
path.poses.push_back(poseStamped);
if(path.poses.size()>=numOfPose)
{
path.poses.erase(path.poses.begin());
}
path_pub.publish(path);
}
void RVIZPath::clearPath()
{
path.poses.clear();
}