Diff: main.cpp

Revision:

6:1508faf1f383

Parent:

3:ab392a9f941d

Child:

7:f71761cac14a

--- a/main.cpp	Fri Jul 20 17:41:31 2018 +0000
+++ b/main.cpp	Fri Jul 20 18:41:20 2018 +0000
@@ -2,6 +2,9 @@
 #include "QEI.h"
 #include "ros.h"
 #include "geometry_msgs/Quaternion.h"
+#include <tf/transform_broadcaster.h>
+#include <nav_msgs/Odometry.h>
+
 
 //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
 QEI leftX(D4, D5, NC, 378,QEI::X2_ENCODING);
@@ -11,23 +14,27 @@
 
 ros::NodeHandle nh;
 geometry_msgs::Quaternion wheelMsg;
+geometry_msgs::Quaternion odom_quat;
 ros::Publisher wheelPub("/odom_msg", &wheelMsg); 
 
+geometry_msgs::TransformStamped odom_trans;
 //rosserial does not like volatile ints
 int leftPosX=0; 
 int leftPosY=0;
 int rightPosX=0; 
 int rightPosY=0;
-
+double theta=0;
+ros::Time current_time;
 int main(){
-    
+
     nh.initNode();
     //nh.advertise(chatter);
     nh.advertise(wheelPub);
     // pc.baud(57600);
-    
+
+    tf::TransformBroadcaster odom_broadcaster;
     while(true){
-    
+    current_time = ros::Time::now();
     leftPosX = leftX.getPulses();
     leftPosY = leftY.getPulses();
     rightPosX = rightX.getPulses();
@@ -37,8 +44,26 @@
     wheelMsg.y = leftPosY;
     wheelMsg.z = rightPosX;
     wheelMsg.w = rightPosY;    
+    wheelPub.publish(&wheelMsg);
     
-    wheelPub.publish(&wheelMsg);
+    odom_quat.x=0;
+    odom_quat.y=0;
+    odom_quat.z=0;
+    odom_quat.w=0;    
+
+    
+    odom_trans.header.stamp= current_time;
+    odom_trans.header.frame_id="mouse1_odom";
+    odom_trans.header.stamp = current_time;
+    odom_trans.header.frame_id = "odom";
+    odom_trans.child_frame_id = "base_link";
+    odom_trans.transform.translation.x = wheelMsg.x;
+    odom_trans.transform.translation.y = wheelMsg.y;
+    odom_trans.transform.translation.z = 0.0;
+    odom_trans.transform.rotation = odom_quat;    
+
+    //send the transform
+    odom_broadcaster.sendTransform(odom_trans);
     nh.spinOnce();
     wait(0.01);
     }