SBD Digital Accelerator Robotics / Mbed 2 deprecated Mouse_ISR_test1

added broadcaster for l.mouse

Dependencies:   QEI mbed ros_lib_kinetic

Fork of Mouse_ISR_broadcaster by SBD Digital Accelerator Robotics

main.cpp@9:e705f028f853, 2018-07-24 (annotated)

Committer:
sreeraj
Date:
Tue Jul 24 21:34:29 2018 +0000
Revision:
9:e705f028f853
Parent:
8:f118cc66e271
Child:
10:52f59f5a9b7d
implemented dead reckoning, but values not reliable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
apriljunio 0:1a95e3b1026a 1 #include "mbed.h"
apriljunio 0:1a95e3b1026a 2 #include "QEI.h"
apriljunio 0:1a95e3b1026a 3 #include "ros.h"
sreeraj 7:f71761cac14a 4 #include "ros/time.h"
sreeraj 7:f71761cac14a 5 #include <tf/tf.h>
sreeraj 8:f118cc66e271 6 #include <math.h>
apriljunio 0:1a95e3b1026a 7 #include "geometry_msgs/Quaternion.h"
sreeraj 6:1508faf1f383 8 #include <tf/transform_broadcaster.h>
sreeraj 6:1508faf1f383 9 #include <nav_msgs/Odometry.h>
sreeraj 7:f71761cac14a 10 #include "geometry_msgs/Vector3.h"
sreeraj 9:e705f028f853 11 #include "iostream"
sreeraj 6:1508faf1f383 12
sreeraj 7:f71761cac14a 13 #define ODOM_TICK_TOPIC "/odom_msg"
sreeraj 7:f71761cac14a 14 #define ODOM_TOPIC "/odom"
sreeraj 7:f71761cac14a 15
sreeraj 7:f71761cac14a 16 #define ODOM_FRAME "odom"
sreeraj 7:f71761cac14a 17 #define BASE_FRAME "base_link"
sreeraj 7:f71761cac14a 18
sreeraj 7:f71761cac14a 19 #define RADTODEG(angleInRadians) ((angleInRadians) * 180.0 / M_PI)
sreeraj 7:f71761cac14a 20
sreeraj 7:f71761cac14a 21 ros::NodeHandle nh;
apriljunio 0:1a95e3b1026a 22
apriljunio 0:1a95e3b1026a 23 //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
apriljunio 3:ab392a9f941d 24 QEI leftX(D4, D5, NC, 378,QEI::X2_ENCODING);
apriljunio 3:ab392a9f941d 25 QEI leftY(D10, D11, NC, 378,QEI::X2_ENCODING);
apriljunio 2:47663f3fec3a 26 QEI rightX(D0, D1, NC, 378,QEI::X2_ENCODING);
apriljunio 2:47663f3fec3a 27 QEI rightY(D2, D3, NC, 378,QEI::X2_ENCODING);
apriljunio 0:1a95e3b1026a 28
sreeraj 7:f71761cac14a 29
apriljunio 0:1a95e3b1026a 30 geometry_msgs::Quaternion wheelMsg;
apriljunio 0:1a95e3b1026a 31 ros::Publisher wheelPub("/odom_msg", &wheelMsg);
apriljunio 0:1a95e3b1026a 32
sreeraj 8:f118cc66e271 33 geometry_msgs::Vector3 odom_base;
sreeraj 8:f118cc66e271 34 ros::Publisher odom_base_pub("/odom_base", &odom_base);
sreeraj 8:f118cc66e271 35
sreeraj 7:f71761cac14a 36 //for odom_pub
sreeraj 7:f71761cac14a 37 //geometry_msgs::Quaternion odom_quat;
sreeraj 6:1508faf1f383 38 geometry_msgs::TransformStamped odom_trans;
sreeraj 8:f118cc66e271 39 geometry_msgs::TransformStamped odom_trans2;
sreeraj 8:f118cc66e271 40
sreeraj 8:f118cc66e271 41
sreeraj 7:f71761cac14a 42 struct OdomValues {
sreeraj 7:f71761cac14a 43 double x;
sreeraj 7:f71761cac14a 44 double y;
sreeraj 7:f71761cac14a 45 double theta;
sreeraj 7:f71761cac14a 46
sreeraj 7:f71761cac14a 47 OdomValues(double x, double y, double theta) {
sreeraj 7:f71761cac14a 48 this->x = x;
sreeraj 7:f71761cac14a 49 this->y = y;
sreeraj 7:f71761cac14a 50 this->theta = theta;
sreeraj 7:f71761cac14a 51 }
sreeraj 7:f71761cac14a 52 };
sreeraj 7:f71761cac14a 53 nav_msgs::Odometry odomMsg;
sreeraj 7:f71761cac14a 54 ros::Publisher odomPub("odom", &odomMsg);
sreeraj 7:f71761cac14a 55 geometry_msgs::TransformStamped odomTransform;
sreeraj 7:f71761cac14a 56 tf::TransformBroadcaster odomBroadcaster;
sreeraj 7:f71761cac14a 57
sreeraj 8:f118cc66e271 58 geometry_msgs::TransformStamped odomTransform2;
sreeraj 8:f118cc66e271 59 tf::TransformBroadcaster odomBroadcaster2;
sreeraj 8:f118cc66e271 60
sreeraj 8:f118cc66e271 61
sreeraj 8:f118cc66e271 62 geometry_msgs::TransformStamped odomTransform3;
sreeraj 8:f118cc66e271 63 tf::TransformBroadcaster odomBroadcaster3;
sreeraj 8:f118cc66e271 64
sreeraj 7:f71761cac14a 65 OdomValues currentOdom(0.0, 0.0, 0.0);
sreeraj 7:f71761cac14a 66
sreeraj 7:f71761cac14a 67 void publishOdometry();
sreeraj 7:f71761cac14a 68
sreeraj 7:f71761cac14a 69
sreeraj 7:f71761cac14a 70
apriljunio 1:7ba9803d4e01 71 //rosserial does not like volatile ints
apriljunio 2:47663f3fec3a 72 int leftPosX=0;
apriljunio 2:47663f3fec3a 73 int leftPosY=0;
apriljunio 2:47663f3fec3a 74 int rightPosX=0;
apriljunio 2:47663f3fec3a 75 int rightPosY=0;
sreeraj 8:f118cc66e271 76
sreeraj 9:e705f028f853 77 int prev_leftX =0;
sreeraj 9:e705f028f853 78 int prev_leftY =0;
sreeraj 9:e705f028f853 79 int prev_rightX=0;
sreeraj 9:e705f028f853 80 int prev_rightY=0;
sreeraj 9:e705f028f853 81 double D= 10;
sreeraj 9:e705f028f853 82 double x_l= -D/2;
sreeraj 9:e705f028f853 83 double y_l=0;
sreeraj 9:e705f028f853 84 double x_r= D/2;
sreeraj 9:e705f028f853 85 double y_r =0;
sreeraj 9:e705f028f853 86 double del_x=0;
sreeraj 9:e705f028f853 87 double del_y=0;
sreeraj 9:e705f028f853 88 double l_l=0;
sreeraj 9:e705f028f853 89 double l_r=0;
sreeraj 9:e705f028f853 90 double x=0;
sreeraj 9:e705f028f853 91 double y=0;
sreeraj 9:e705f028f853 92 double theta=0;
sreeraj 9:e705f028f853 93 double del_theta=0;
sreeraj 9:e705f028f853 94
sreeraj 9:e705f028f853 95 int signum(double val){
sreeraj 9:e705f028f853 96 if (val > 0) return 1;
sreeraj 9:e705f028f853 97 if (val < 0) return -1;
sreeraj 9:e705f028f853 98 return 0;
sreeraj 9:e705f028f853 99 }
sreeraj 8:f118cc66e271 100
apriljunio 0:1a95e3b1026a 101 int main(){
sreeraj 6:1508faf1f383 102
apriljunio 0:1a95e3b1026a 103 nh.initNode();
apriljunio 0:1a95e3b1026a 104 nh.advertise(wheelPub);
sreeraj 8:f118cc66e271 105 nh.advertise(odom_base_pub);
sreeraj 7:f71761cac14a 106
sreeraj 7:f71761cac14a 107 nh.advertise(odomPub);
sreeraj 7:f71761cac14a 108 odomBroadcaster.init(nh);
sreeraj 8:f118cc66e271 109 odomBroadcaster2.init(nh);
sreeraj 8:f118cc66e271 110 odomBroadcaster3.init(nh);
apriljunio 2:47663f3fec3a 111 // pc.baud(57600);
sreeraj 6:1508faf1f383 112
apriljunio 0:1a95e3b1026a 113 while(true){
apriljunio 2:47663f3fec3a 114 leftPosX = leftX.getPulses();
apriljunio 2:47663f3fec3a 115 leftPosY = leftY.getPulses();
apriljunio 2:47663f3fec3a 116 rightPosX = rightX.getPulses();
apriljunio 2:47663f3fec3a 117 rightPosY = rightY.getPulses();
apriljunio 0:1a95e3b1026a 118
sreeraj 7:f71761cac14a 119 wheelMsg.x= leftPosX/10;
sreeraj 7:f71761cac14a 120 wheelMsg.y = leftPosY/10;
sreeraj 7:f71761cac14a 121 wheelMsg.z = rightPosX/10;
sreeraj 8:f118cc66e271 122 wheelMsg.w = rightPosY/10;
sreeraj 8:f118cc66e271 123
sreeraj 9:e705f028f853 124
sreeraj 9:e705f028f853 125 //calculate pose
sreeraj 9:e705f028f853 126 int x_l_bar= leftPosX - prev_leftX;
sreeraj 9:e705f028f853 127 int y_l_bar= leftPosY - prev_leftY;
sreeraj 9:e705f028f853 128 int x_r_bar= rightPosX - prev_rightX;
sreeraj 9:e705f028f853 129 int y_r_bar= rightPosY - prev_rightY;
sreeraj 9:e705f028f853 130
sreeraj 9:e705f028f853 131 double alpha_l= atan(double(y_l_bar/x_l_bar));
sreeraj 9:e705f028f853 132 double alpha_r= atan(double(y_r_bar/x_r_bar));
sreeraj 9:e705f028f853 133 cout<<"alpha_l"<<alpha_l;
sreeraj 9:e705f028f853 134
sreeraj 9:e705f028f853 135
sreeraj 9:e705f028f853 136 if(alpha_l== 0 || 3.141592){
sreeraj 9:e705f028f853 137 l_l=x_l_bar;
sreeraj 9:e705f028f853 138 }
sreeraj 9:e705f028f853 139 else{
sreeraj 9:e705f028f853 140 l_l= y_l_bar/sin(alpha_l);
sreeraj 9:e705f028f853 141 }
sreeraj 9:e705f028f853 142 if(alpha_r== 0 || 3.141592){
sreeraj 9:e705f028f853 143 l_r=x_r_bar;
sreeraj 9:e705f028f853 144 }
sreeraj 9:e705f028f853 145 else{
sreeraj 9:e705f028f853 146 l_r= y_r_bar/sin(alpha_r);
sreeraj 9:e705f028f853 147 }
sreeraj 9:e705f028f853 148 double gamma= abs( alpha_l - alpha_r);
sreeraj 9:e705f028f853 149 double temp1 =y_r-y_l;
sreeraj 9:e705f028f853 150 del_theta= signum(temp1)*(1/D)* sqrt(pow(l_l,2) + pow(l_r,2) - (2*cos(gamma)*l_l*l_r));
sreeraj 9:e705f028f853 151 if (del_theta==0){
sreeraj 9:e705f028f853 152 del_theta=1;
sreeraj 9:e705f028f853 153 }
sreeraj 9:e705f028f853 154 else{
sreeraj 9:e705f028f853 155 continue;
sreeraj 9:e705f028f853 156 }
sreeraj 9:e705f028f853 157
sreeraj 9:e705f028f853 158 double r_l= l_l/abs(del_theta);
sreeraj 9:e705f028f853 159 double r_r= l_r/abs(del_theta);
sreeraj 9:e705f028f853 160 x_r= r_r* (sin(alpha_r +del_theta) - sin(alpha_r)) * signum(del_theta) +(D/2);
sreeraj 9:e705f028f853 161 y_r= r_r*(cos(alpha_r)-cos(alpha_r +del_theta))* signum(del_theta);
sreeraj 9:e705f028f853 162 x_l = r_l*(sin(alpha_l+del_theta) - sin(alpha_l))*signum(del_theta)-(D/2);
sreeraj 9:e705f028f853 163 y_l=r_l*(cos(alpha_l)-cos(alpha_l+del_theta))*signum(del_theta);
sreeraj 9:e705f028f853 164
sreeraj 9:e705f028f853 165 del_x= (x_r+x_l)*0.5;
sreeraj 9:e705f028f853 166 del_y=(y_r+y_l)*0.5;
sreeraj 9:e705f028f853 167
sreeraj 9:e705f028f853 168 x= x+ (sqrt(pow(del_x,2)+pow(del_y,2))*cos(theta+atan(del_y/del_x)));
sreeraj 9:e705f028f853 169 y= y+ (sqrt(pow(del_x,2)+pow(del_y,2))*sin(theta+atan(del_y/del_x)));
sreeraj 9:e705f028f853 170 theta=theta+del_theta;
sreeraj 9:e705f028f853 171
sreeraj 9:e705f028f853 172 //odom_base.x = (leftPosX+rightPosX)*0.05;
sreeraj 9:e705f028f853 173 //odom_base.y=(leftPosY + rightPosY)*0.05;
sreeraj 9:e705f028f853 174 odom_base.x =x;
sreeraj 9:e705f028f853 175 odom_base.y=y;
sreeraj 9:e705f028f853 176 odom_base.z=theta;
sreeraj 9:e705f028f853 177 /*if(rightPosX- leftPosX !=0){
sreeraj 8:f118cc66e271 178 odom_base.z= atan(double(rightPosY-leftPosY)/double(rightPosX- leftPosX))*(180/3.14);
sreeraj 8:f118cc66e271 179 }
sreeraj 8:f118cc66e271 180 else{
sreeraj 8:f118cc66e271 181 odom_base.z=0.0;
sreeraj 9:e705f028f853 182 }*/
sreeraj 9:e705f028f853 183
sreeraj 8:f118cc66e271 184 odom_base_pub.publish(&odom_base);
sreeraj 6:1508faf1f383 185 wheelPub.publish(&wheelMsg);
sreeraj 7:f71761cac14a 186
sreeraj 7:f71761cac14a 187
sreeraj 7:f71761cac14a 188 publishOdometry();
sreeraj 9:e705f028f853 189
sreeraj 9:e705f028f853 190 prev_leftX =leftPosX;
sreeraj 9:e705f028f853 191 prev_leftY =leftPosY;
sreeraj 9:e705f028f853 192 prev_rightX=rightPosX;
sreeraj 9:e705f028f853 193 prev_rightY=rightPosY;
sreeraj 9:e705f028f853 194
apriljunio 0:1a95e3b1026a 195 nh.spinOnce();
sreeraj 9:e705f028f853 196 wait(.15);
apriljunio 0:1a95e3b1026a 197 }
sreeraj 7:f71761cac14a 198 }
sreeraj 7:f71761cac14a 199 void publishOdometry() {
sreeraj 7:f71761cac14a 200 // Set initial header values
sreeraj 7:f71761cac14a 201 odomTransform.header.stamp = nh.now();
sreeraj 7:f71761cac14a 202 odomTransform.header.frame_id = ODOM_FRAME;
sreeraj 8:f118cc66e271 203 odomTransform.child_frame_id = "mouse1";
sreeraj 8:f118cc66e271 204
sreeraj 8:f118cc66e271 205 odomTransform2.header.stamp = nh.now();
sreeraj 8:f118cc66e271 206 odomTransform2.header.frame_id = ODOM_FRAME;
sreeraj 8:f118cc66e271 207 odomTransform2.child_frame_id = "mouse2";
sreeraj 8:f118cc66e271 208
sreeraj 8:f118cc66e271 209 odomTransform3.header.stamp = nh.now();
sreeraj 8:f118cc66e271 210 odomTransform3.header.frame_id = ODOM_FRAME;
sreeraj 8:f118cc66e271 211 odomTransform3.child_frame_id = "mouse_base";
sreeraj 7:f71761cac14a 212
sreeraj 7:f71761cac14a 213 odomMsg.header.stamp = odomTransform.header.stamp;
sreeraj 7:f71761cac14a 214 odomMsg.header.frame_id = odomTransform.header.frame_id;
sreeraj 7:f71761cac14a 215 odomMsg.child_frame_id = odomTransform.child_frame_id;
sreeraj 7:f71761cac14a 216
sreeraj 7:f71761cac14a 217 geometry_msgs::Quaternion rotation = tf::createQuaternionFromYaw(currentOdom.theta);
sreeraj 7:f71761cac14a 218
sreeraj 9:e705f028f853 219 odomTransform.transform.translation.x = x_l;
sreeraj 9:e705f028f853 220 odomTransform.transform.translation.y = y_l;
sreeraj 7:f71761cac14a 221 odomTransform.transform.translation.z = 0.0;
sreeraj 7:f71761cac14a 222 odomTransform.transform.rotation = rotation;
sreeraj 8:f118cc66e271 223
sreeraj 9:e705f028f853 224 odomTransform2.transform.translation.x = x_r;
sreeraj 9:e705f028f853 225 odomTransform2.transform.translation.y = y_r;
sreeraj 8:f118cc66e271 226 odomTransform2.transform.translation.z = 0.0;
sreeraj 8:f118cc66e271 227 odomTransform2.transform.rotation = rotation;
sreeraj 8:f118cc66e271 228
sreeraj 9:e705f028f853 229 odomTransform3.transform.translation.x = x;
sreeraj 9:e705f028f853 230 odomTransform3.transform.translation.y = y;
sreeraj 8:f118cc66e271 231 odomTransform3.transform.translation.z = 0.0;
sreeraj 8:f118cc66e271 232 odomTransform3.transform.rotation = rotation;
sreeraj 7:f71761cac14a 233
sreeraj 9:e705f028f853 234 odomMsg.pose.pose.position.x = x;
sreeraj 9:e705f028f853 235 odomMsg.pose.pose.position.y =y;
sreeraj 7:f71761cac14a 236 odomMsg.pose.pose.position.z = 0.0;
sreeraj 7:f71761cac14a 237 odomMsg.pose.pose.orientation = rotation;
sreeraj 7:f71761cac14a 238
sreeraj 7:f71761cac14a 239 // Publish result
sreeraj 7:f71761cac14a 240 odomPub.publish(&odomMsg);
sreeraj 7:f71761cac14a 241 odomBroadcaster.sendTransform(odomTransform);
sreeraj 8:f118cc66e271 242 odomBroadcaster2.sendTransform(odomTransform2);
sreeraj 8:f118cc66e271 243 odomBroadcaster3.sendTransform(odomTransform3);
sreeraj 8:f118cc66e271 244
sreeraj 7:f71761cac14a 245 }
sreeraj 9:e705f028f853 246
sreeraj 9:e705f028f853 247