Diff: main.cpp

Revision:

5:e674692f6d15

Parent:

4:f87a415beec4

--- a/main.cpp	Fri Jul 20 21:24:23 2018 +0000
+++ b/main.cpp	Mon Jul 23 14:38:21 2018 +0000
@@ -12,13 +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;
+
 
 geometry_msgs::Quaternion odom_quat;
 geometry_msgs::TransformStamped odom_trans;
 tf::TransformBroadcaster odom_broadcaster;
-
+ros::Publisher wheelPub("/odom_msg", &wheelMsg); 
 
 //rosserial does not like volatile ints
 int leftPosX=0; 
@@ -30,10 +30,11 @@
 ros::Time current_time;
 
 int main(){
-    
     nh.initNode();
+    odom_broadcaster.init(nh);
     //nh.advertise(chatter);
-    //nh.advertise(wheelPub);
+    nh.advertise(wheelPub);
+    //nh.advertise(odom_quat);
     // pc.baud(57600);
     
 
@@ -46,10 +47,10 @@
     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;
@@ -58,7 +59,7 @@
  
     
     odom_trans.header.stamp= current_time;
-    odom_trans.header.frame_id="mouse1_odom";
+    odom_trans.header.frame_id="world";
     odom_trans.header.stamp = current_time;
     odom_trans.child_frame_id = "base_link";
     odom_trans.transform.translation.x = rightPosX;
@@ -69,8 +70,8 @@
     //send the transform
     odom_broadcaster.sendTransform(odom_trans);
     
-    //wheelPub.publish(&wheelMsg);
+    wheelPub.publish(&wheelMsg);
     nh.spinOnce();
-    wait(0.01);
+    wait(1000);
     }
 }
\ No newline at end of file