ros-path

#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 <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
 
 
nav_msgs::Path  path;
geometry_msgs::PoseStamped first_pose_stamped;
bool first_odom = true;
 
ros::Publisher  path_pub, angle_pub, com_pub;
ros::Subscriber odomSub, com_sub;
ros::Subscriber odom_raw_Sub;

 void com_callback(const nav_msgs::Odometry::ConstPtr& odom) {
     nav_msgs::Odometry tmp_odom;
     tmp_odom = *odom;
     tf::Quaternion q = tf::createQuaternionFromRPY( 0, 0, (tf::getYaw(odom->pose.pose.orientation) + 0.17) );
     tmp_odom.pose.pose.orientation.w = q.getW();
     tmp_odom.pose.pose.orientation.x = q.getX();
     tmp_odom.pose.pose.orientation.y = q.getY();
     tmp_odom.pose.pose.orientation.z = q.getZ();

     com_pub.publish(tmp_odom);
 }
 
 void odomCallback(const nav_msgs::Odometry::ConstPtr& odom)
 {

    if (first_odom) {
        first_odom = false;
        first_pose_stamped.pose.position.x = odom->pose.pose.position.x;
        first_pose_stamped.pose.position.y = odom->pose.pose.position.y;
        return;
    }

    geometry_msgs::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x = first_pose_stamped.pose.position.x;
    this_pose_stamped.pose.position.y = first_pose_stamped.pose.position.y;


 
    this_pose_stamped.header.stamp = ros::Time::now();
    this_pose_stamped.header.frame_id = "base_odom";
 
    path.poses.push_back(this_pose_stamped);
 
    path.header.stamp = ros::Time::now();
    path.header.frame_id="base_odom";
    path_pub.publish(path);

    geometry_msgs::PoseWithCovarianceStamped tmp_pose;
    tmp_pose.header.stamp = ros::Time::now();
    tmp_pose.header.frame_id = "world";
    tmp_pose.pose.pose.position = this_pose_stamped.pose.position;
    tmp_pose.pose.pose.orientation = odom->pose.pose.orientation;
    angle_pub.publish(tmp_pose);

 }
 
 
int main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath");
 
    ros::NodeHandle ph;
 
    path_pub = ph.advertise<nav_msgs::Path>("trajectory",10, true);
    angle_pub = ph.advertise<geometry_msgs::PoseWithCovarianceStamped>("/angle_pose", 100, true);
    com_pub = ph.advertise<nav_msgs::Odometry>("/odom_modify", 100, true);
    odomSub  = ph.subscribe<nav_msgs::Odometry>("/odom", 10, odomCallback);
    com_sub = ph.subscribe<nav_msgs::Odometry>("/odom", 10, com_callback);
 
    ros::Rate loop_rate(50);
 
    while (ros::ok())
    {
        ros::spinOnce();               // check for incoming messages
        loop_rate.sleep();
    }
 
    return 0;
}