w/ ROSserial Publisher

Dependencies:   QEI mbed ros_lib_kinetic

main.cpp

Committer:
apriljunio
Date:
2018-07-23
Revision:
5:4eb9b5e03f02
Parent:
1:7ba9803d4e01

File content as of revision 5:4eb9b5e03f02:

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

//PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
QEI leftEncoder(D6, D7, NC, 378,QEI::X4_ENCODING);
QEI rightEncoder(D8, D9, NC, 378,QEI::X4_ENCODING);

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

//rosserial does not like volatile ints
int leftPulseA=0; 
int rightPulseA=0;

int main(){
    
    nh.initNode();
    //nh.advertise(chatter);
    nh.advertise(wheelPub);
    // pc.baud(57600);
    
    while(true){
    
    leftPulseA=leftEncoder.getPulses();
    rightPulseA = rightEncoder.getPulses();
    
    wheelMsg.x= leftPulseA;
    wheelMsg.y = rightPulseA;//timeL;
    wheelMsg.z = 0.0;
    wheelMsg.w = 0.0;//timeR;    
    
    wheelPub.publish(&wheelMsg);
    nh.spinOnce();
    wait(0.01);
    }
}