w/ ROSserial Publisher
Dependencies: QEI mbed ros_lib_kinetic
main.cpp
- Committer:
- apriljunio
- Date:
- 2018-07-19
- Revision:
- 2:47663f3fec3a
- Parent:
- 1:7ba9803d4e01
- Child:
- 3:ab392a9f941d
File content as of revision 2:47663f3fec3a:
#include "mbed.h" #include "QEI.h" #include "ros.h" #include "geometry_msgs/Quaternion.h" //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT) QEI leftX(D6, D7, NC, 378,QEI::X2_ENCODING); QEI leftY(D8, D9, NC, 378,QEI::X2_ENCODING); QEI rightX(D0, D1, NC, 378,QEI::X2_ENCODING); QEI rightY(D2, D3, NC, 378,QEI::X2_ENCODING); ros::NodeHandle nh; geometry_msgs::Quaternion wheelMsg; ros::Publisher wheelPub("/odom_msg", &wheelMsg); //rosserial does not like volatile ints int leftPosX=0; int leftPosY=0; int rightPosX=0; int rightPosY=0; int main(){ nh.initNode(); //nh.advertise(chatter); nh.advertise(wheelPub); // pc.baud(57600); while(true){ leftPosX = leftX.getPulses(); leftPosY = leftY.getPulses(); rightPosX = rightX.getPulses(); rightPosY = rightY.getPulses(); wheelMsg.x= leftPosX; wheelMsg.y = leftPosY; wheelMsg.z = rightPosX; wheelMsg.w = rightPosY; wheelPub.publish(&wheelMsg); nh.spinOnce(); wait(0.01); } }