Diff: main.cpp

Revision:

10:52f59f5a9b7d

Parent:

9:e705f028f853

--- a/main.cpp	Tue Jul 24 21:34:29 2018 +0000
+++ b/main.cpp	Fri Aug 17 17:32:07 2018 +0000
@@ -4,19 +4,12 @@
 #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>
 #include "geometry_msgs/Vector3.h"
 #include "iostream"
-
-#define ODOM_TICK_TOPIC "/odom_msg"
-#define ODOM_TOPIC "/odom"
+#include "std_msgs/Float64.h"
 
-#define ODOM_FRAME "odom"
-#define BASE_FRAME "base_link"
-
-#define RADTODEG(angleInRadians) ((angleInRadians) * 180.0 / M_PI)
 
 ros::NodeHandle nh;
 
@@ -30,41 +23,11 @@
 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;
-
+std_msgs::Float64 lwheelMsg;
+ros::Publisher lwheelPub("/Lodom_msg", &lwheelMsg); 
 
-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;
-
-geometry_msgs::TransformStamped odomTransform2;
-tf::TransformBroadcaster odomBroadcaster2;
-
-
-geometry_msgs::TransformStamped odomTransform3;
-tf::TransformBroadcaster odomBroadcaster3;
-
-OdomValues currentOdom(0.0, 0.0, 0.0);
-
-void publishOdometry();
+std_msgs::Float64 rwheelMsg;
+ros::Publisher rwheelPub("/Rodom_msg", &rwheelMsg);
 
 
 
@@ -74,42 +37,13 @@
 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(){
 
     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);
-
+    nh.advertise(lwheelPub);
+    nh.advertise(rwheelPub);
     while(true){
     leftPosX = leftX.getPulses();
     leftPosY = leftY.getPulses();
@@ -120,128 +54,17 @@
     wheelMsg.y = leftPosY/10;
     wheelMsg.z = rightPosX/10;
     wheelMsg.w = rightPosY/10; 
-    
-    
-    //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);
+    lwheelMsg.data=wheelMsg.y;
+    rwheelMsg.data= wheelMsg.w;
+    //lwheelMsg.data= (sqrt(pow((wheelMsg.x),2)+pow((wheelMsg.y),2)));
+    //rwheelMsg.data= sqrt(pow(wheelMsg.z,2)+pow(wheelMsg.w,2));
 
 
-    publishOdometry();
-    
-    prev_leftX =leftPosX;
-    prev_leftY =leftPosY;
-    prev_rightX=rightPosX;
-    prev_rightY=rightPosY;
+    wheelPub.publish(&wheelMsg);
+    lwheelPub.publish(&lwheelMsg);
+    rwheelPub.publish(&rwheelMsg);
     
     nh.spinOnce();
-    wait(.15);
+    wait(.35);
     }
 }
-void publishOdometry() {
-    // Set initial header values
-    odomTransform.header.stamp = nh.now();
-    odomTransform.header.frame_id = ODOM_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;
-    odomMsg.child_frame_id = odomTransform.child_frame_id;
-
-    geometry_msgs::Quaternion rotation = tf::createQuaternionFromYaw(currentOdom.theta);
-    
-    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 = x_r;
-    odomTransform2.transform.translation.y = y_r;
-    odomTransform2.transform.translation.z = 0.0;
-    odomTransform2.transform.rotation = rotation;
-    
-    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 = x;
-    odomMsg.pose.pose.position.y =y;
-    odomMsg.pose.pose.position.z = 0.0;
-    odomMsg.pose.pose.orientation = rotation;
-
-    // Publish result
-    odomPub.publish(&odomMsg);
-    odomBroadcaster.sendTransform(odomTransform);
-    odomBroadcaster2.sendTransform(odomTransform2);
-    odomBroadcaster3.sendTransform(odomTransform3);
-    
-}
-
-