Diff: main.cpp

Revision:

8:f118cc66e271

Parent:

7:f71761cac14a

Child:

9:e705f028f853

--- a/main.cpp	Mon Jul 23 20:56:32 2018 +0000
+++ b/main.cpp	Tue Jul 24 16:41:55 2018 +0000
@@ -3,6 +3,7 @@
 #include "ros.h"
 #include "ros/time.h"
 #include <tf/tf.h>
+#include <math.h>
 #include "geometry_msgs/Quaternion.h"
 #include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
@@ -28,9 +29,15 @@
 geometry_msgs::Quaternion wheelMsg;
 ros::Publisher wheelPub("/odom_msg", &wheelMsg); 
 
+geometry_msgs::Vector3 odom_base;
+ros::Publisher odom_base_pub("/odom_base", &odom_base); 
+
 //for odom_pub
 //geometry_msgs::Quaternion odom_quat;
 geometry_msgs::TransformStamped odom_trans;
+geometry_msgs::TransformStamped odom_trans2;
+
+
 struct OdomValues {
     double x;
     double y;
@@ -47,6 +54,13 @@
 geometry_msgs::TransformStamped odomTransform;
 tf::TransformBroadcaster odomBroadcaster;
 
+geometry_msgs::TransformStamped odomTransform2;
+tf::TransformBroadcaster odomBroadcaster2;
+
+
+geometry_msgs::TransformStamped odomTransform3;
+tf::TransformBroadcaster odomBroadcaster3;
+
 OdomValues currentOdom(0.0, 0.0, 0.0);
 
 void publishOdometry();
@@ -58,15 +72,18 @@
 int leftPosY=0;
 int rightPosX=0; 
 int rightPosY=0;
-//double theta=0;
-//ros::Time current_time;
+
+
 int main(){
 
     nh.initNode();
     nh.advertise(wheelPub);
+    nh.advertise(odom_base_pub);
     
     nh.advertise(odomPub);
     odomBroadcaster.init(nh);
+    odomBroadcaster2.init(nh);
+    odomBroadcaster3.init(nh);
     // pc.baud(57600);
 
     while(true){
@@ -78,41 +95,38 @@
     wheelMsg.x= leftPosX/10;
     wheelMsg.y = leftPosY/10;
     wheelMsg.z = rightPosX/10;
-    wheelMsg.w = rightPosY/10;    
+    wheelMsg.w = rightPosY/10; 
+    
+    odom_base.x = (leftPosX+rightPosX)*0.05;
+    odom_base.y=(leftPosY + rightPosY)*0.05;   
+    if(rightPosX- leftPosX !=0){
+        odom_base.z= atan(double(rightPosY-leftPosY)/double(rightPosX- leftPosX))*(180/3.14);
+    }
+    else{
+        odom_base.z=0.0;
+    }
+    odom_base_pub.publish(&odom_base);
     wheelPub.publish(&wheelMsg);
 
 
-/*    current_time = nh.now();
-    
-    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);
-    */
     publishOdometry();
     nh.spinOnce();
-    wait(.1);
+    wait(.08);
     }
 }
 void publishOdometry() {
     // Set initial header values
     odomTransform.header.stamp = nh.now();
     odomTransform.header.frame_id = ODOM_FRAME;
-    odomTransform.child_frame_id = BASE_FRAME;
+    odomTransform.child_frame_id = "mouse1";
+        
+    odomTransform2.header.stamp = nh.now();
+    odomTransform2.header.frame_id = ODOM_FRAME;
+    odomTransform2.child_frame_id = "mouse2";
+    
+    odomTransform3.header.stamp = nh.now();
+    odomTransform3.header.frame_id = ODOM_FRAME;
+    odomTransform3.child_frame_id = "mouse_base";
 
     odomMsg.header.stamp = odomTransform.header.stamp;
     odomMsg.header.frame_id = odomTransform.header.frame_id;
@@ -124,6 +138,16 @@
     odomTransform.transform.translation.y = wheelMsg.y;
     odomTransform.transform.translation.z = 0.0;
     odomTransform.transform.rotation = rotation;
+    
+    odomTransform2.transform.translation.x = wheelMsg.z;
+    odomTransform2.transform.translation.y = wheelMsg.w;
+    odomTransform2.transform.translation.z = 0.0;
+    odomTransform2.transform.rotation = rotation;
+    
+    odomTransform3.transform.translation.x = odom_base.x;
+    odomTransform3.transform.translation.y = odom_base.y;
+    odomTransform3.transform.translation.z = 0.0;
+    odomTransform3.transform.rotation = rotation;
 
     odomMsg.pose.pose.position.x = wheelMsg.x;
     odomMsg.pose.pose.position.y = wheelMsg.y;
@@ -133,4 +157,7 @@
     // Publish result
     odomPub.publish(&odomMsg);
     odomBroadcaster.sendTransform(odomTransform);
+    odomBroadcaster2.sendTransform(odomTransform2);
+    odomBroadcaster3.sendTransform(odomTransform3);
+    
 }