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@8:f118cc66e271, 2018-07-24 (annotated)

Committer:
sreeraj
Date:
Tue Jul 24 16:41:55 2018 +0000
Revision:
8:f118cc66e271
Parent:
7:f71761cac14a
Child:
9:e705f028f853
added tf for mice and base

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 6:1508faf1f383 11
sreeraj 7:f71761cac14a 12 #define ODOM_TICK_TOPIC "/odom_msg"
sreeraj 7:f71761cac14a 13 #define ODOM_TOPIC "/odom"
sreeraj 7:f71761cac14a 14
sreeraj 7:f71761cac14a 15 #define ODOM_FRAME "odom"
sreeraj 7:f71761cac14a 16 #define BASE_FRAME "base_link"
sreeraj 7:f71761cac14a 17
sreeraj 7:f71761cac14a 18 #define RADTODEG(angleInRadians) ((angleInRadians) * 180.0 / M_PI)
sreeraj 7:f71761cac14a 19
sreeraj 7:f71761cac14a 20 ros::NodeHandle nh;
apriljunio 0:1a95e3b1026a 21
apriljunio 0:1a95e3b1026a 22 //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
apriljunio 3:ab392a9f941d 23 QEI leftX(D4, D5, NC, 378,QEI::X2_ENCODING);
apriljunio 3:ab392a9f941d 24 QEI leftY(D10, D11, NC, 378,QEI::X2_ENCODING);
apriljunio 2:47663f3fec3a 25 QEI rightX(D0, D1, NC, 378,QEI::X2_ENCODING);
apriljunio 2:47663f3fec3a 26 QEI rightY(D2, D3, NC, 378,QEI::X2_ENCODING);
apriljunio 0:1a95e3b1026a 27
sreeraj 7:f71761cac14a 28
apriljunio 0:1a95e3b1026a 29 geometry_msgs::Quaternion wheelMsg;
apriljunio 0:1a95e3b1026a 30 ros::Publisher wheelPub("/odom_msg", &wheelMsg);
apriljunio 0:1a95e3b1026a 31
sreeraj 8:f118cc66e271 32 geometry_msgs::Vector3 odom_base;
sreeraj 8:f118cc66e271 33 ros::Publisher odom_base_pub("/odom_base", &odom_base);
sreeraj 8:f118cc66e271 34
sreeraj 7:f71761cac14a 35 //for odom_pub
sreeraj 7:f71761cac14a 36 //geometry_msgs::Quaternion odom_quat;
sreeraj 6:1508faf1f383 37 geometry_msgs::TransformStamped odom_trans;
sreeraj 8:f118cc66e271 38 geometry_msgs::TransformStamped odom_trans2;
sreeraj 8:f118cc66e271 39
sreeraj 8:f118cc66e271 40
sreeraj 7:f71761cac14a 41 struct OdomValues {
sreeraj 7:f71761cac14a 42 double x;
sreeraj 7:f71761cac14a 43 double y;
sreeraj 7:f71761cac14a 44 double theta;
sreeraj 7:f71761cac14a 45
sreeraj 7:f71761cac14a 46 OdomValues(double x, double y, double theta) {
sreeraj 7:f71761cac14a 47 this->x = x;
sreeraj 7:f71761cac14a 48 this->y = y;
sreeraj 7:f71761cac14a 49 this->theta = theta;
sreeraj 7:f71761cac14a 50 }
sreeraj 7:f71761cac14a 51 };
sreeraj 7:f71761cac14a 52 nav_msgs::Odometry odomMsg;
sreeraj 7:f71761cac14a 53 ros::Publisher odomPub("odom", &odomMsg);
sreeraj 7:f71761cac14a 54 geometry_msgs::TransformStamped odomTransform;
sreeraj 7:f71761cac14a 55 tf::TransformBroadcaster odomBroadcaster;
sreeraj 7:f71761cac14a 56
sreeraj 8:f118cc66e271 57 geometry_msgs::TransformStamped odomTransform2;
sreeraj 8:f118cc66e271 58 tf::TransformBroadcaster odomBroadcaster2;
sreeraj 8:f118cc66e271 59
sreeraj 8:f118cc66e271 60
sreeraj 8:f118cc66e271 61 geometry_msgs::TransformStamped odomTransform3;
sreeraj 8:f118cc66e271 62 tf::TransformBroadcaster odomBroadcaster3;
sreeraj 8:f118cc66e271 63
sreeraj 7:f71761cac14a 64 OdomValues currentOdom(0.0, 0.0, 0.0);
sreeraj 7:f71761cac14a 65
sreeraj 7:f71761cac14a 66 void publishOdometry();
sreeraj 7:f71761cac14a 67
sreeraj 7:f71761cac14a 68
sreeraj 7:f71761cac14a 69
apriljunio 1:7ba9803d4e01 70 //rosserial does not like volatile ints
apriljunio 2:47663f3fec3a 71 int leftPosX=0;
apriljunio 2:47663f3fec3a 72 int leftPosY=0;
apriljunio 2:47663f3fec3a 73 int rightPosX=0;
apriljunio 2:47663f3fec3a 74 int rightPosY=0;
sreeraj 8:f118cc66e271 75
sreeraj 8:f118cc66e271 76
apriljunio 0:1a95e3b1026a 77 int main(){
sreeraj 6:1508faf1f383 78
apriljunio 0:1a95e3b1026a 79 nh.initNode();
apriljunio 0:1a95e3b1026a 80 nh.advertise(wheelPub);
sreeraj 8:f118cc66e271 81 nh.advertise(odom_base_pub);
sreeraj 7:f71761cac14a 82
sreeraj 7:f71761cac14a 83 nh.advertise(odomPub);
sreeraj 7:f71761cac14a 84 odomBroadcaster.init(nh);
sreeraj 8:f118cc66e271 85 odomBroadcaster2.init(nh);
sreeraj 8:f118cc66e271 86 odomBroadcaster3.init(nh);
apriljunio 2:47663f3fec3a 87 // pc.baud(57600);
sreeraj 6:1508faf1f383 88
apriljunio 0:1a95e3b1026a 89 while(true){
apriljunio 2:47663f3fec3a 90 leftPosX = leftX.getPulses();
apriljunio 2:47663f3fec3a 91 leftPosY = leftY.getPulses();
apriljunio 2:47663f3fec3a 92 rightPosX = rightX.getPulses();
apriljunio 2:47663f3fec3a 93 rightPosY = rightY.getPulses();
apriljunio 0:1a95e3b1026a 94
sreeraj 7:f71761cac14a 95 wheelMsg.x= leftPosX/10;
sreeraj 7:f71761cac14a 96 wheelMsg.y = leftPosY/10;
sreeraj 7:f71761cac14a 97 wheelMsg.z = rightPosX/10;
sreeraj 8:f118cc66e271 98 wheelMsg.w = rightPosY/10;
sreeraj 8:f118cc66e271 99
sreeraj 8:f118cc66e271 100 odom_base.x = (leftPosX+rightPosX)*0.05;
sreeraj 8:f118cc66e271 101 odom_base.y=(leftPosY + rightPosY)*0.05;
sreeraj 8:f118cc66e271 102 if(rightPosX- leftPosX !=0){
sreeraj 8:f118cc66e271 103 odom_base.z= atan(double(rightPosY-leftPosY)/double(rightPosX- leftPosX))*(180/3.14);
sreeraj 8:f118cc66e271 104 }
sreeraj 8:f118cc66e271 105 else{
sreeraj 8:f118cc66e271 106 odom_base.z=0.0;
sreeraj 8:f118cc66e271 107 }
sreeraj 8:f118cc66e271 108 odom_base_pub.publish(&odom_base);
sreeraj 6:1508faf1f383 109 wheelPub.publish(&wheelMsg);
sreeraj 7:f71761cac14a 110
sreeraj 7:f71761cac14a 111
sreeraj 7:f71761cac14a 112 publishOdometry();
apriljunio 0:1a95e3b1026a 113 nh.spinOnce();
sreeraj 8:f118cc66e271 114 wait(.08);
apriljunio 0:1a95e3b1026a 115 }
sreeraj 7:f71761cac14a 116 }
sreeraj 7:f71761cac14a 117 void publishOdometry() {
sreeraj 7:f71761cac14a 118 // Set initial header values
sreeraj 7:f71761cac14a 119 odomTransform.header.stamp = nh.now();
sreeraj 7:f71761cac14a 120 odomTransform.header.frame_id = ODOM_FRAME;
sreeraj 8:f118cc66e271 121 odomTransform.child_frame_id = "mouse1";
sreeraj 8:f118cc66e271 122
sreeraj 8:f118cc66e271 123 odomTransform2.header.stamp = nh.now();
sreeraj 8:f118cc66e271 124 odomTransform2.header.frame_id = ODOM_FRAME;
sreeraj 8:f118cc66e271 125 odomTransform2.child_frame_id = "mouse2";
sreeraj 8:f118cc66e271 126
sreeraj 8:f118cc66e271 127 odomTransform3.header.stamp = nh.now();
sreeraj 8:f118cc66e271 128 odomTransform3.header.frame_id = ODOM_FRAME;
sreeraj 8:f118cc66e271 129 odomTransform3.child_frame_id = "mouse_base";
sreeraj 7:f71761cac14a 130
sreeraj 7:f71761cac14a 131 odomMsg.header.stamp = odomTransform.header.stamp;
sreeraj 7:f71761cac14a 132 odomMsg.header.frame_id = odomTransform.header.frame_id;
sreeraj 7:f71761cac14a 133 odomMsg.child_frame_id = odomTransform.child_frame_id;
sreeraj 7:f71761cac14a 134
sreeraj 7:f71761cac14a 135 geometry_msgs::Quaternion rotation = tf::createQuaternionFromYaw(currentOdom.theta);
sreeraj 7:f71761cac14a 136
sreeraj 7:f71761cac14a 137 odomTransform.transform.translation.x = wheelMsg.x;
sreeraj 7:f71761cac14a 138 odomTransform.transform.translation.y = wheelMsg.y;
sreeraj 7:f71761cac14a 139 odomTransform.transform.translation.z = 0.0;
sreeraj 7:f71761cac14a 140 odomTransform.transform.rotation = rotation;
sreeraj 8:f118cc66e271 141
sreeraj 8:f118cc66e271 142 odomTransform2.transform.translation.x = wheelMsg.z;
sreeraj 8:f118cc66e271 143 odomTransform2.transform.translation.y = wheelMsg.w;
sreeraj 8:f118cc66e271 144 odomTransform2.transform.translation.z = 0.0;
sreeraj 8:f118cc66e271 145 odomTransform2.transform.rotation = rotation;
sreeraj 8:f118cc66e271 146
sreeraj 8:f118cc66e271 147 odomTransform3.transform.translation.x = odom_base.x;
sreeraj 8:f118cc66e271 148 odomTransform3.transform.translation.y = odom_base.y;
sreeraj 8:f118cc66e271 149 odomTransform3.transform.translation.z = 0.0;
sreeraj 8:f118cc66e271 150 odomTransform3.transform.rotation = rotation;
sreeraj 7:f71761cac14a 151
sreeraj 7:f71761cac14a 152 odomMsg.pose.pose.position.x = wheelMsg.x;
sreeraj 7:f71761cac14a 153 odomMsg.pose.pose.position.y = wheelMsg.y;
sreeraj 7:f71761cac14a 154 odomMsg.pose.pose.position.z = 0.0;
sreeraj 7:f71761cac14a 155 odomMsg.pose.pose.orientation = rotation;
sreeraj 7:f71761cac14a 156
sreeraj 7:f71761cac14a 157 // Publish result
sreeraj 7:f71761cac14a 158 odomPub.publish(&odomMsg);
sreeraj 7:f71761cac14a 159 odomBroadcaster.sendTransform(odomTransform);
sreeraj 8:f118cc66e271 160 odomBroadcaster2.sendTransform(odomTransform2);
sreeraj 8:f118cc66e271 161 odomBroadcaster3.sendTransform(odomTransform3);
sreeraj 8:f118cc66e271 162
sreeraj 7:f71761cac14a 163 }