odom->base_link

来源:互联网 发布:手机数据分区损坏修复 编辑:程序博客网 时间:2024/05/17 01:00

tf::TransformBroadcaster odom_broadcaster;

ros::Time current_time,last_time;

current_time = ros::Time::now();
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(t);
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
        
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0;
odom_trans.transform.rotation = odom_quat;
odom_broadcaster.sendTransform(odom_trans);


nav_msgs::Odometry odom;
   odom.header.stamp = current_time;
odom.header.frame_id = "odom";
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;
odom_pub.publish(odom);

0 0
原创粉丝点击