【ROS】将odom里程计数据转换为path路径消息

订阅底盘odom数据，转换为path数据；

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>#include <nav_msgs/Odometry.h>#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>nav_msgs::Path  path;ros::Publisher  path_pub;
ros::Subscriber odomSub;
ros::Subscriber odom_raw_Sub;void odomCallback(const nav_msgs::Odometry::ConstPtr& odom){geometry_msgs::PoseStamped this_pose_stamped;this_pose_stamped.pose.position.x = odom->pose.pose.position.x;this_pose_stamped.pose.position.y = odom->pose.pose.position.y;this_pose_stamped.pose.orientation = odom->pose.pose.orientation;this_pose_stamped.header.stamp = ros::Time::now();this_pose_stamped.header.frame_id = "odom";path.poses.push_back(this_pose_stamped);path.header.stamp = ros::Time::now();path.header.frame_id="odom";path_pub.publish(path);printf("path_pub ");printf("odom %.3lf %.3lf\n",odom->pose.pose.position.x,odom->pose.pose.position.y);}int main (int argc, char **argv)
{ros::init (argc, argv, "showpath");ros::NodeHandle ph;path_pub = ph.advertise<nav_msgs::Path>("trajectory",10, true);odomSub  = ph.subscribe<nav_msgs::Odometry>("/odom", 10, odomCallback);odom_raw_Sub  = ph.subscribe<nav_msgs::Odometry>("/odom_raw", 10, odomCallback);ros::Rate loop_rate(50);while (ros::ok()){ros::spinOnce();               // check for incoming messagesloop_rate.sleep();}return 0;
}int trajectory_test (void )
{ros::NodeHandle ph;ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("trajectory_test",1, true);ros::Time current_time, last_time;current_time = ros::Time::now();last_time = ros::Time::now();nav_msgs::Path path;//nav_msgs::Path path;// path.header.stamp=current_time;// path.header.frame_id="odom";double x = 0.0;double y = 0.0;double th = 0.0;double vx = 0.1;double vy = -0.1;double vth = 0.1;ros::Rate loop_rate(1);while (ros::ok()){current_time = ros::Time::now();//compute odometry in a typical way given the velocities of the robotdouble dt = (current_time - last_time).toSec();double delta_x = (vx * cos(th) - vy * sin(th)) * dt;double delta_y = (vx * sin(th) + vy * cos(th)) * dt;double delta_th = vth * dt;x += delta_x;y += delta_y;th += delta_th;geometry_msgs::PoseStamped this_pose_stamped;this_pose_stamped.pose.position.x = x;this_pose_stamped.pose.position.y = y;geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(th);this_pose_stamped.pose.orientation.x = goal_quat.x;this_pose_stamped.pose.orientation.y = goal_quat.y;this_pose_stamped.pose.orientation.z = goal_quat.z;this_pose_stamped.pose.orientation.w = goal_quat.w;this_pose_stamped.header.stamp=current_time;this_pose_stamped.header.frame_id="odom";path.poses.push_back(this_pose_stamped);path.header.stamp=current_time;path.header.frame_id="odom";path_pub.publish(path);ros::spinOnce();               // check for incoming messageslast_time = current_time;loop_rate.sleep();}return 0;
}

【ROS】将odom里程计数据转换为path路径消息相关推荐

底盘编码数据解算ROS的odom数据
参考:读取编码器的计数脉冲转化成ROS的odom数据对这些式子没理解,如何由底盘速度数据发布odom数据. dx = cos(dth) * dxy_avedy = -sin(dth) * dxy_a ...
ros之odom发布
ros之odom的发布 odom就是所谓的里程计,是根据你所获得的线速度,角速度,积分获得机器人实际走的距离,里程计存在累计误差,用于后面多传感器输入,为建图,导航提供参数. odom最重要的是积分和 ...
odom 里程计轨迹的代码
在 C++ 中预测 odom 里程计轨迹的代码可能需要使用一些库来帮助处理数据和进行计算.以下是一个示例,它使用 Eigen 库来表示和操作矩阵和向量,以及使用 ROS (Robot Operatin ...
Java把科学计数法转换为字符串
为什么80%的码农都做不了架构师?>>> /*** 把科学计数法转换为字符串* 1.64607507E8(转化为164607507).1.6.0.0.0* @param str ...
卡尔曼滤波器预测 odom 里程计轨迹的代码
卡尔曼滤波器预测 odom 里程计轨迹的代码首先,你需要包含必要的头文件,包括 Eigen 库,这是一个线性代数库,你可以使用它来表示矩阵和向量,以及 cmath 库,它包含数学函数. #inc ...
ASP.NET根路径的获取和将Web站点下的绝对路径转换为虚拟路径的两种方案
ASP.NET 根路径的获取 private string _ApplicationPath; /// <summary> /// ...
ASP.net 1.1 中相对路径转换为绝对路径
It's just a code snippet of mine. /// <summary> /// 将相对路径转换为绝对路径 /// 注:最终得到任何场合都可以正常链接的路径(不论将应 ...
Python将绝对路径转换为相对路径方法实例
本篇文章主要讲解,python绝对路径转换为相对路径的方法作者:任聪聪通过python转换为相对路径,实际上是有多种方法的,一般可以通过正则替换盘符,replace进行替换等等. 本次主要讲解利用 ...
sudo找不到命令：修改sudo的PATH路径
sudo有时候会出现找不到命令,而明明PATH路径下包含该命令,让人疑惑.其实出现这种情况的原因,主要是因为当 sudo以管理权限执行命令的时候,linux将PATH环境变量进行了重置,当然这主要是因 ...

【ROS】将odom里程计数据转换为path路径消息

【ROS】将odom里程计数据转换为path路径消息相关推荐

最新文章

热门文章