added broadcaster for l.mouse

Dependencies:   QEI mbed ros_lib_kinetic

Fork of Mouse_ISR_broadcaster by SBD Digital Accelerator Robotics

main.cpp

Committer:
apriljunio
Date:
2018-07-16
Revision:
0:1a95e3b1026a
Child:
1:7ba9803d4e01

File content as of revision 0:1a95e3b1026a:

#include "mbed.h"
#include "QEI.h"
#include "ros.h"
#include "geometry_msgs/Quaternion.h"
//#include "std_msgs/String.h"

//PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
QEI leftEncoder(D3, D12, NC, 378);
QEI rightEncoder(D2, D13, NC, 378);

Timer tL, tR; 

//Serial pc(USBTX, USBRX); 

ros::NodeHandle nh;
geometry_msgs::Quaternion wheelMsg;
ros::Publisher wheelPub("/odom_msg", &wheelMsg); 

//std_msgs::String str_msg; 
//ros::Publisher chatter("chatter", &str_msg);

//char hello[13]="Hello World";

int leftPulseA=0; 
int rightPulseA=0;

unsigned long timeL =0 ; 
unsigned long timeR=0; 

int main(){
    
    nh.initNode();
    //nh.advertise(chatter);
    nh.advertise(wheelPub);
   // pc.baud(57600);

    
    while(true){
    
    leftPulseA=leftEncoder.getPulses();
    rightPulseA = rightEncoder.getPulses();
    timeL = tL.read();
    timeR = tL.read();
    
    wheelMsg.x= leftPulseA;
    wheelMsg.y = timeL;;
    wheelMsg.z = rightPulseA;
    wheelMsg.w = timeR;    
    
    
    //pc.printf("Left pulses %i \n", leftPulseA);
    //pc.printf("Right pulses %i \n", rightPulseA);
    
    //str_msg.data=hello;
    //chatter.publish(&str_msg);
    wheelPub.publish(&wheelMsg);
    nh.spinOnce();
    wait(0.01);
    }
}