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@7:f71761cac14a, 2018-07-23 (annotated)

Committer:
sreeraj
Date:
Mon Jul 23 20:56:32 2018 +0000
Revision:
7:f71761cac14a
Parent:
6:1508faf1f383
Child:
8:f118cc66e271
tf publisher for 1 mouse added

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