update

nodes/odom_publisher.cpp

@@ -1,57 +1,80 @@
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <signal.h>
 
-volatile sig_atomic_t flag = 0;
+int main(int argc, char** argv){
+  ros::init(argc, argv, "odometry_publisher");
 
-void signal_handler(int signum) {
-  flag = 1;
-}
-
-void odomCallback(const nav_msgs::Odometry::ConstPtr& msg, tf::TransformBroadcaster& odom_broadcaster) {
-  // Create transform message
-  geometry_msgs::TransformStamped odom_trans;
-  odom_trans.header.stamp = msg->header.stamp;
-  odom_trans.header.frame_id = msg->header.frame_id;
-  odom_trans.child_frame_id = msg->child_frame_id;
-
-  // Copy translation and rotation
-  odom_trans.transform.translation.x = msg->pose.pose.position.x;
-  odom_trans.transform.translation.y = msg->pose.pose.position.y;
-  odom_trans.transform.translation.z = msg->pose.pose.position.z;
-  odom_trans.transform.rotation = msg->pose.pose.orientation;
-
-  // Publish the transform
-  odom_broadcaster.sendTransform(odom_trans);
-}
-
-int main(int argc, char** argv) {
-  ros::init(argc, argv, "tf_broadcaster");
-  ros::NodeHandle nh;
-
-  // Register the signal handler
-  signal(SIGINT, signal_handler);
-
-  // Create the transform broadcaster
+  ros::NodeHandle n;
+  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
   tf::TransformBroadcaster odom_broadcaster;
 
-  // Subscribe to the /odom topic
-  ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>(
-      "/odom", 10,
-      boost::bind(odomCallback, _1, boost::ref(odom_broadcaster)));
+  double x = 0.0;
+  double y = 0.0;
+  double th = 0.0;
 
-  ROS_INFO("TF broadcaster node running...");
+  double vx = 0.1;
+  double vy = -0.1;
+  double vth = 0.1;
 
-  // Spin until the node is stopped
-  ros::Rate rate(10.0); // Adjust rate if necessary
-  while (ros::ok() && !flag) {
-    ros::spinOnce();
-    rate.sleep();
+  ros::Time current_time, last_time;
+  current_time = ros::Time::now();
+  last_time = ros::Time::now();
+
+  ros::Rate r(1.0);
+  while(n.ok()){
+
+    ros::spinOnce();               // check for incoming messages
+    current_time = ros::Time::now();
+
+    //compute odometry in a typical way given the velocities of the robot
+    double 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;
+
+    //since all odometry is 6DOF we'll need a quaternion created from yaw
+    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
+
+    //first, we'll publish the transform over tf
+    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.0;
+    odom_trans.transform.rotation = odom_quat;
+
+    //send the transform
+    odom_broadcaster.sendTransform(odom_trans);
+
+    //next, we'll publish the odometry message over ROS
+    nav_msgs::Odometry odom;
+    odom.header.stamp = current_time;
+    odom.header.frame_id = "odom";
+
+    //set the position
+    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;
+
+    //set the velocity
+    odom.child_frame_id = "base_link";
+    odom.twist.twist.linear.x = vx;
+    odom.twist.twist.linear.y = vy;
+    odom.twist.twist.angular.z = vth;
+
+    //publish the message
+    odom_pub.publish(odom);
+
+    last_time = current_time;
+    r.sleep();
   }
-
-  ROS_INFO("TF broadcaster node shutting down.");
-  ros::shutdown();
-  return 0;
-}
+}