Diff: main.cpp

Revision:

9:e705f028f853

Parent:

8:f118cc66e271

Child:

10:52f59f5a9b7d

--- a/main.cpp	Tue Jul 24 16:41:55 2018 +0000
+++ b/main.cpp	Tue Jul 24 21:34:29 2018 +0000
@@ -8,6 +8,7 @@
 #include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
 #include "geometry_msgs/Vector3.h"
+#include "iostream"
 
 #define ODOM_TICK_TOPIC "/odom_msg"
 #define ODOM_TOPIC "/odom"
@@ -73,6 +74,29 @@
 int rightPosX=0; 
 int rightPosY=0;
 
+int prev_leftX =0;
+int prev_leftY =0;
+int prev_rightX=0;
+int prev_rightY=0;
+double D= 10;
+double x_l= -D/2;
+double y_l=0;
+double x_r= D/2;
+double y_r =0;
+double del_x=0;
+double del_y=0;
+double l_l=0;
+double l_r=0;
+double x=0;
+double y=0;
+double theta=0;
+double del_theta=0;
+
+int signum(double val){
+    if (val > 0) return 1;
+    if (val < 0) return -1;
+    return 0;
+}
 
 int main(){
 
@@ -97,21 +121,79 @@
     wheelMsg.z = rightPosX/10;
     wheelMsg.w = rightPosY/10; 
     
-    odom_base.x = (leftPosX+rightPosX)*0.05;
-    odom_base.y=(leftPosY + rightPosY)*0.05;   
-    if(rightPosX- leftPosX !=0){
+    
+    //calculate pose
+    int x_l_bar= leftPosX - prev_leftX;
+    int y_l_bar= leftPosY - prev_leftY;
+    int x_r_bar= rightPosX - prev_rightX;
+    int y_r_bar= rightPosY - prev_rightY;
+    
+    double alpha_l= atan(double(y_l_bar/x_l_bar));
+    double alpha_r= atan(double(y_r_bar/x_r_bar));
+    cout<<"alpha_l"<<alpha_l;
+    
+    
+    if(alpha_l== 0 || 3.141592){
+        l_l=x_l_bar;
+    }
+    else{
+        l_l= y_l_bar/sin(alpha_l);
+    }
+    if(alpha_r== 0 || 3.141592){
+        l_r=x_r_bar;
+    }
+    else{
+        l_r= y_r_bar/sin(alpha_r);
+    }
+    double gamma= abs( alpha_l - alpha_r);
+    double temp1 =y_r-y_l;
+    del_theta= signum(temp1)*(1/D)* sqrt(pow(l_l,2) + pow(l_r,2) - (2*cos(gamma)*l_l*l_r));
+    if (del_theta==0){
+        del_theta=1;
+    }
+    else{
+        continue;
+    }
+        
+    double r_l= l_l/abs(del_theta);
+    double r_r= l_r/abs(del_theta);
+    x_r= r_r* (sin(alpha_r +del_theta) - sin(alpha_r)) * signum(del_theta) +(D/2);
+    y_r= r_r*(cos(alpha_r)-cos(alpha_r +del_theta))* signum(del_theta);
+    x_l = r_l*(sin(alpha_l+del_theta) - sin(alpha_l))*signum(del_theta)-(D/2);
+    y_l=r_l*(cos(alpha_l)-cos(alpha_l+del_theta))*signum(del_theta);
+    
+    del_x= (x_r+x_l)*0.5;
+    del_y=(y_r+y_l)*0.5;
+    
+    x= x+ (sqrt(pow(del_x,2)+pow(del_y,2))*cos(theta+atan(del_y/del_x)));
+    y= y+ (sqrt(pow(del_x,2)+pow(del_y,2))*sin(theta+atan(del_y/del_x)));
+    theta=theta+del_theta;    
+    
+    //odom_base.x = (leftPosX+rightPosX)*0.05;
+    //odom_base.y=(leftPosY + rightPosY)*0.05; 
+    odom_base.x =x;
+    odom_base.y=y;  
+    odom_base.z=theta;
+    /*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);
 
 
     publishOdometry();
+    
+    prev_leftX =leftPosX;
+    prev_leftY =leftPosY;
+    prev_rightX=rightPosX;
+    prev_rightY=rightPosY;
+    
     nh.spinOnce();
-    wait(.08);
+    wait(.15);
     }
 }
 void publishOdometry() {
@@ -134,23 +216,23 @@
 
     geometry_msgs::Quaternion rotation = tf::createQuaternionFromYaw(currentOdom.theta);
     
-    odomTransform.transform.translation.x = wheelMsg.x;
-    odomTransform.transform.translation.y = wheelMsg.y;
+    odomTransform.transform.translation.x = x_l;
+    odomTransform.transform.translation.y = y_l;
     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.x = x_r;
+    odomTransform2.transform.translation.y = y_r;
     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.x = x;
+    odomTransform3.transform.translation.y = 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;
+    odomMsg.pose.pose.position.x = x;
+    odomMsg.pose.pose.position.y =y;
     odomMsg.pose.pose.position.z = 0.0;
     odomMsg.pose.pose.orientation = rotation;
 
@@ -161,3 +243,5 @@
     odomBroadcaster3.sendTransform(odomTransform3);
     
 }
+
+