Diff: main.cpp

Revision:

7:f71761cac14a

Parent:

6:1508faf1f383

--- a/main.cpp	Fri Jul 20 18:41:20 2018 +0000
+++ b/main.cpp	Mon Jul 23 20:56:32 2018 +0000
@@ -1,10 +1,22 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "ros.h"
+#include "ros/time.h"
+#include <tf/tf.h>
 #include "geometry_msgs/Quaternion.h"
 #include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
+#include "geometry_msgs/Vector3.h"
 
+#define ODOM_TICK_TOPIC "/odom_msg"
+#define ODOM_TOPIC "/odom"
+
+#define ODOM_FRAME "odom"
+#define BASE_FRAME "base_link"
+
+#define RADTODEG(angleInRadians) ((angleInRadians) * 180.0 / M_PI)
+
+ros::NodeHandle nh;
 
 //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
 QEI leftX(D4, D5, NC, 378,QEI::X2_ENCODING);
@@ -12,39 +24,65 @@
 QEI rightX(D0, D1, NC, 378,QEI::X2_ENCODING);
 QEI rightY(D2, D3, NC, 378,QEI::X2_ENCODING);
 
-ros::NodeHandle nh;
+
 geometry_msgs::Quaternion wheelMsg;
-geometry_msgs::Quaternion odom_quat;
 ros::Publisher wheelPub("/odom_msg", &wheelMsg); 
 
+//for odom_pub
+//geometry_msgs::Quaternion odom_quat;
 geometry_msgs::TransformStamped odom_trans;
+struct OdomValues {
+    double x;
+    double y;
+    double theta;
+    
+    OdomValues(double x, double y, double theta) {
+        this->x = x;
+        this->y = y;
+        this->theta = theta;    
+    }
+};
+nav_msgs::Odometry odomMsg;
+ros::Publisher odomPub("odom", &odomMsg);
+geometry_msgs::TransformStamped odomTransform;
+tf::TransformBroadcaster odomBroadcaster;
+
+OdomValues currentOdom(0.0, 0.0, 0.0);
+
+void publishOdometry();
+
+
+
 //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;
+//double theta=0;
+//ros::Time current_time;
 int main(){
 
     nh.initNode();
-    //nh.advertise(chatter);
     nh.advertise(wheelPub);
+    
+    nh.advertise(odomPub);
+    odomBroadcaster.init(nh);
     // pc.baud(57600);
 
-    tf::TransformBroadcaster odom_broadcaster;
     while(true){
-    current_time = ros::Time::now();
     leftPosX = leftX.getPulses();
     leftPosY = leftY.getPulses();
     rightPosX = rightX.getPulses();
     rightPosY = rightY.getPulses();
     
-    wheelMsg.x= leftPosX;
-    wheelMsg.y = leftPosY;
-    wheelMsg.z = rightPosX;
-    wheelMsg.w = rightPosY;    
+    wheelMsg.x= leftPosX/10;
+    wheelMsg.y = leftPosY/10;
+    wheelMsg.z = rightPosX/10;
+    wheelMsg.w = rightPosY/10;    
     wheelPub.publish(&wheelMsg);
+
+
+/*    current_time = nh.now();
     
     odom_quat.x=0;
     odom_quat.y=0;
@@ -64,7 +102,35 @@
 
     //send the transform
     odom_broadcaster.sendTransform(odom_trans);
+    */
+    publishOdometry();
     nh.spinOnce();
-    wait(0.01);
+    wait(.1);
     }
-}
\ No newline at end of file
+}
+void publishOdometry() {
+    // Set initial header values
+    odomTransform.header.stamp = nh.now();
+    odomTransform.header.frame_id = ODOM_FRAME;
+    odomTransform.child_frame_id = BASE_FRAME;
+
+    odomMsg.header.stamp = odomTransform.header.stamp;
+    odomMsg.header.frame_id = odomTransform.header.frame_id;
+    odomMsg.child_frame_id = odomTransform.child_frame_id;
+
+    geometry_msgs::Quaternion rotation = tf::createQuaternionFromYaw(currentOdom.theta);
+    
+    odomTransform.transform.translation.x = wheelMsg.x;
+    odomTransform.transform.translation.y = wheelMsg.y;
+    odomTransform.transform.translation.z = 0.0;
+    odomTransform.transform.rotation = rotation;
+
+    odomMsg.pose.pose.position.x = wheelMsg.x;
+    odomMsg.pose.pose.position.y = wheelMsg.y;
+    odomMsg.pose.pose.position.z = 0.0;
+    odomMsg.pose.pose.orientation = rotation;
+
+    // Publish result
+    odomPub.publish(&odomMsg);
+    odomBroadcaster.sendTransform(odomTransform);
+}