Diff: main.cpp

Revision:

4:f87a415beec4

Parent:

3:ab392a9f941d

Child:

5:e674692f6d15

--- a/main.cpp	Fri Jul 20 17:41:31 2018 +0000
+++ b/main.cpp	Fri Jul 20 21:24:23 2018 +0000
@@ -2,6 +2,8 @@
 #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);
@@ -10,8 +12,13 @@
 QEI rightY(D2, D3, NC, 378,QEI::X2_ENCODING);
 
 ros::NodeHandle nh;
-geometry_msgs::Quaternion wheelMsg;
-ros::Publisher wheelPub("/odom_msg", &wheelMsg); 
+//geometry_msgs::Quaternion wheelMsg;
+//ros::Publisher wheelPub("/odom_msg", &wheelMsg); 
+
+geometry_msgs::Quaternion odom_quat;
+geometry_msgs::TransformStamped odom_trans;
+tf::TransformBroadcaster odom_broadcaster;
+
 
 //rosserial does not like volatile ints
 int leftPosX=0; 
@@ -19,26 +26,50 @@
 int rightPosX=0; 
 int rightPosY=0;
 
+double theta=0;
+ros::Time current_time;
+
 int main(){
     
     nh.initNode();
     //nh.advertise(chatter);
-    nh.advertise(wheelPub);
+    //nh.advertise(wheelPub);
     // pc.baud(57600);
     
+
     while(true){
     
+    current_time = nh.now();
+    
     leftPosX = leftX.getPulses();
     leftPosY = leftY.getPulses();
     rightPosX = rightX.getPulses();
     rightPosY = rightY.getPulses();
     
-    wheelMsg.x= leftPosX;
+    /*wheelMsg.x= leftPosX;
     wheelMsg.y = leftPosY;
     wheelMsg.z = rightPosX;
-    wheelMsg.w = rightPosY;    
+    wheelMsg.w = rightPosY;    */
+    
+    odom_quat.x=0;
+    odom_quat.y=0;
+    odom_quat.z=0;
+    odom_quat.w=0;    
+ 
     
-    wheelPub.publish(&wheelMsg);
+    odom_trans.header.stamp= current_time;
+    odom_trans.header.frame_id="mouse1_odom";
+    odom_trans.header.stamp = current_time;
+    odom_trans.child_frame_id = "base_link";
+    odom_trans.transform.translation.x = rightPosX;
+    odom_trans.transform.translation.y = rightPosY;
+    odom_trans.transform.translation.z = 0.0;
+    odom_trans.transform.rotation = odom_quat;    
+ 
+    //send the transform
+    odom_broadcaster.sendTransform(odom_trans);
+    
+    //wheelPub.publish(&wheelMsg);
     nh.spinOnce();
     wait(0.01);
     }