39 #include <nav_msgs/Odometry.h> 41 int main(
int argc,
char** argv){
42 ros::init(argc, argv,
"odometry_publisher");
65 double dt = (current_time - last_time).toSec();
66 double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
67 double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
68 double delta_th = vth * dt;
78 geometry_msgs::TransformStamped odom_trans;
79 odom_trans.header.stamp = current_time;
80 odom_trans.header.frame_id =
"odom";
81 odom_trans.child_frame_id =
"base_link";
83 odom_trans.transform.translation.x = x;
84 odom_trans.transform.translation.y = y;
85 odom_trans.transform.translation.z = 0.0;
86 odom_trans.transform.rotation = odom_quat;
89 odom_broadcaster.sendTransform(odom_trans);
92 nav_msgs::Odometry odom;
93 odom.header.stamp = current_time;
94 odom.header.frame_id =
"odom";
95 odom.child_frame_id =
"base_link";
98 odom.pose.pose.position.x = x;
99 odom.pose.pose.position.y = y;
100 odom.pose.pose.position.z = 0.0;
101 odom.pose.pose.orientation = odom_quat;
104 odom.twist.twist.linear.x = vx;
105 odom.twist.twist.linear.y = vy;
106 odom.twist.twist.angular.z = vth;
111 last_time = current_time;
int main(int argc, char **argv)
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)