VNH ROSSERIAL PUB+SUB
Dependencies: Motordriver QEI mbed ros_lib_kinetic
Fork of Test by
main.cpp
- Committer:
- apriljunio
- Date:
- 2018-07-16
- Revision:
- 2:82249618b6bc
- Parent:
- 1:5f2cb6bc0a8e
- Child:
- 3:9202fa788d3c
File content as of revision 2:82249618b6bc:
#include "mbed.h" #include "motordriver.h" #include "ros.h" #include "geometry_msgs/Vector3.h" #include "QEI.h" //#include "geometry_msgs/Quaternion.h" #include "std_msgs/String.h" //pwm, fwd, rev, can brake Motor motorLeft(D9,D5,D4, 1); Motor motorRight(D10, D7, D8, 1); //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT) //QEI leftEncoder(D3, D12, NC, 378); //QEI rightEncoder(D2, D13, NC, 378); 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"; void messageCb( const geometry_msgs::Vector3& msg) { motorLeft.speed(msg.x); motorRight.speed(msg.y); } ros::Subscriber<geometry_msgs::Vector3> sub("/motor/command", &messageCb ); //int leftPulseA=0; //int rightPulseA=0; int main() { nh.initNode(); nh.subscribe(sub); nh.advertise(chatter); //nh.advertise(wheelPub); while (true) { // leftPulseA=leftEncoder.getPulses(); // rightPulseA = rightEncoder.getPulses(); /* wheelMsg.x= leftPulseA; wheelMsg.y = rightPulseA; wheelMsg.z = 0.0; wheelMsg.w = 0.0; */ //wheelPub.publish(&wheelMsg); str_msg.data=hello; chatter.publish(&str_msg); nh.spinOnce(); wait_ms(1); } }