-
Notifications
You must be signed in to change notification settings - Fork 4
/
msgs_conversion.cpp
68 lines (52 loc) · 1.67 KB
/
msgs_conversion.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
55
56
57
58
59
60
61
62
63
64
65
66
67
68
#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/Odometry.h"
#include <string>
#define ODOM_COV 0.005
#define MAIN_LOOP_RATE 20
ros::Publisher *odom_pub_ptr;
std::string tf_prefix_;
bool has_prefix;
void poseCallback(const geometry_msgs::PoseStamped &pose_msg)
{
nav_msgs::Odometry odom_msg;
odom_msg.pose.pose.orientation.x = pose_msg.pose.orientation.x;
odom_msg.pose.pose.orientation.y = pose_msg.pose.orientation.y;
odom_msg.pose.pose.orientation.z = pose_msg.pose.orientation.z;
odom_msg.pose.pose.orientation.w = pose_msg.pose.orientation.w;
odom_msg.pose.pose.position.x = pose_msg.pose.position.x;
odom_msg.pose.pose.position.y = pose_msg.pose.position.y;
odom_msg.pose.pose.position.z = pose_msg.pose.position.z;
odom_msg.pose.covariance[0] = ODOM_COV;
odom_msg.pose.covariance[7] = ODOM_COV;
odom_msg.pose.covariance[14] = ODOM_COV;
odom_msg.pose.covariance[21] = ODOM_COV;
odom_msg.pose.covariance[28] = ODOM_COV;
odom_msg.pose.covariance[35] = ODOM_COV;
odom_msg.header = pose_msg.header;
if (has_prefix)
{
odom_msg.header.frame_id = tf_prefix_ + "/odom";
}
else
{
odom_msg.header.frame_id = "odom";
}
odom_pub_ptr->publish(odom_msg);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "msgs_conversion");
ros::NodeHandle n;
has_prefix = ros::param::get("~tf_prefix", tf_prefix_);
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom/wheel", 1);
odom_pub_ptr = &odom_pub;
ros::Subscriber pose_sub = n.subscribe("pose", 1000, poseCallback);
ros::Rate loop_rate(MAIN_LOOP_RATE);
while (ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}