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@4:f87a415beec4, 2018-07-20 (annotated)

Committer:
apriljunio
Date:
Fri Jul 20 21:24:23 2018 +0000
Revision:
4:f87a415beec4
Parent:
3:ab392a9f941d
Child:
5:e674692f6d15
Transform broadcaster for RVIZ 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"
apriljunio 0:1a95e3b1026a 4 #include "geometry_msgs/Quaternion.h"
apriljunio 4:f87a415beec4 5 #include <tf/transform_broadcaster.h>
apriljunio 4:f87a415beec4 6 #include <nav_msgs/Odometry.h>
apriljunio 0:1a95e3b1026a 7
apriljunio 0:1a95e3b1026a 8 //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
apriljunio 3:ab392a9f941d 9 QEI leftX(D4, D5, NC, 378,QEI::X2_ENCODING);
apriljunio 3:ab392a9f941d 10 QEI leftY(D10, D11, NC, 378,QEI::X2_ENCODING);
apriljunio 2:47663f3fec3a 11 QEI rightX(D0, D1, NC, 378,QEI::X2_ENCODING);
apriljunio 2:47663f3fec3a 12 QEI rightY(D2, D3, NC, 378,QEI::X2_ENCODING);
apriljunio 0:1a95e3b1026a 13
apriljunio 0:1a95e3b1026a 14 ros::NodeHandle nh;
apriljunio 4:f87a415beec4 15 //geometry_msgs::Quaternion wheelMsg;
apriljunio 4:f87a415beec4 16 //ros::Publisher wheelPub("/odom_msg", &wheelMsg);
apriljunio 4:f87a415beec4 17
apriljunio 4:f87a415beec4 18 geometry_msgs::Quaternion odom_quat;
apriljunio 4:f87a415beec4 19 geometry_msgs::TransformStamped odom_trans;
apriljunio 4:f87a415beec4 20 tf::TransformBroadcaster odom_broadcaster;
apriljunio 4:f87a415beec4 21
apriljunio 0:1a95e3b1026a 22
apriljunio 1:7ba9803d4e01 23 //rosserial does not like volatile ints
apriljunio 2:47663f3fec3a 24 int leftPosX=0;
apriljunio 2:47663f3fec3a 25 int leftPosY=0;
apriljunio 2:47663f3fec3a 26 int rightPosX=0;
apriljunio 2:47663f3fec3a 27 int rightPosY=0;
apriljunio 0:1a95e3b1026a 28
apriljunio 4:f87a415beec4 29 double theta=0;
apriljunio 4:f87a415beec4 30 ros::Time current_time;
apriljunio 4:f87a415beec4 31
apriljunio 0:1a95e3b1026a 32 int main(){
apriljunio 0:1a95e3b1026a 33
apriljunio 0:1a95e3b1026a 34 nh.initNode();
apriljunio 0:1a95e3b1026a 35 //nh.advertise(chatter);
apriljunio 4:f87a415beec4 36 //nh.advertise(wheelPub);
apriljunio 2:47663f3fec3a 37 // pc.baud(57600);
apriljunio 0:1a95e3b1026a 38
apriljunio 4:f87a415beec4 39
apriljunio 0:1a95e3b1026a 40 while(true){
apriljunio 0:1a95e3b1026a 41
apriljunio 4:f87a415beec4 42 current_time = nh.now();
apriljunio 4:f87a415beec4 43
apriljunio 2:47663f3fec3a 44 leftPosX = leftX.getPulses();
apriljunio 2:47663f3fec3a 45 leftPosY = leftY.getPulses();
apriljunio 2:47663f3fec3a 46 rightPosX = rightX.getPulses();
apriljunio 2:47663f3fec3a 47 rightPosY = rightY.getPulses();
apriljunio 0:1a95e3b1026a 48
apriljunio 4:f87a415beec4 49 /*wheelMsg.x= leftPosX;
apriljunio 2:47663f3fec3a 50 wheelMsg.y = leftPosY;
apriljunio 2:47663f3fec3a 51 wheelMsg.z = rightPosX;
apriljunio 4:f87a415beec4 52 wheelMsg.w = rightPosY; */
apriljunio 4:f87a415beec4 53
apriljunio 4:f87a415beec4 54 odom_quat.x=0;
apriljunio 4:f87a415beec4 55 odom_quat.y=0;
apriljunio 4:f87a415beec4 56 odom_quat.z=0;
apriljunio 4:f87a415beec4 57 odom_quat.w=0;
apriljunio 4:f87a415beec4 58
apriljunio 0:1a95e3b1026a 59
apriljunio 4:f87a415beec4 60 odom_trans.header.stamp= current_time;
apriljunio 4:f87a415beec4 61 odom_trans.header.frame_id="mouse1_odom";
apriljunio 4:f87a415beec4 62 odom_trans.header.stamp = current_time;
apriljunio 4:f87a415beec4 63 odom_trans.child_frame_id = "base_link";
apriljunio 4:f87a415beec4 64 odom_trans.transform.translation.x = rightPosX;
apriljunio 4:f87a415beec4 65 odom_trans.transform.translation.y = rightPosY;
apriljunio 4:f87a415beec4 66 odom_trans.transform.translation.z = 0.0;
apriljunio 4:f87a415beec4 67 odom_trans.transform.rotation = odom_quat;
apriljunio 4:f87a415beec4 68
apriljunio 4:f87a415beec4 69 //send the transform
apriljunio 4:f87a415beec4 70 odom_broadcaster.sendTransform(odom_trans);
apriljunio 4:f87a415beec4 71
apriljunio 4:f87a415beec4 72 //wheelPub.publish(&wheelMsg);
apriljunio 0:1a95e3b1026a 73 nh.spinOnce();
apriljunio 0:1a95e3b1026a 74 wait(0.01);
apriljunio 0:1a95e3b1026a 75 }
apriljunio 0:1a95e3b1026a 76 }