VNH ROSSERIAL PUB+SUB
Dependencies: Motordriver QEI mbed ros_lib_kinetic
Fork of Test by
main.cpp@3:9202fa788d3c, 2018-07-17 (annotated)
- Committer:
- apriljunio
- Date:
- Tue Jul 17 15:18:55 2018 +0000
- Revision:
- 3:9202fa788d3c
- Parent:
- 2:82249618b6bc
- Child:
- 4:55882b6d5d9a
Added time stamps and change flags
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
apriljunio | 0:99538487421b | 1 | #include "mbed.h" |
apriljunio | 0:99538487421b | 2 | #include "motordriver.h" |
apriljunio | 0:99538487421b | 3 | #include "ros.h" |
apriljunio | 0:99538487421b | 4 | #include "geometry_msgs/Vector3.h" |
apriljunio | 2:82249618b6bc | 5 | #include "QEI.h" |
apriljunio | 3:9202fa788d3c | 6 | #include "geometry_msgs/Quaternion.h" |
apriljunio | 3:9202fa788d3c | 7 | #include "ros/time.h" |
apriljunio | 3:9202fa788d3c | 8 | //#include "std_msgs/String.h" |
apriljunio | 3:9202fa788d3c | 9 | |
apriljunio | 3:9202fa788d3c | 10 | ros:: NodeHandle nh; |
apriljunio | 0:99538487421b | 11 | |
apriljunio | 0:99538487421b | 12 | //pwm, fwd, rev, can brake |
apriljunio | 0:99538487421b | 13 | Motor motorLeft(D9,D5,D4, 1); |
apriljunio | 0:99538487421b | 14 | Motor motorRight(D10, D7, D8, 1); |
apriljunio | 0:99538487421b | 15 | |
apriljunio | 2:82249618b6bc | 16 | //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT) |
apriljunio | 3:9202fa788d3c | 17 | QEI leftEncoder(D3, D12, NC, 378); |
apriljunio | 3:9202fa788d3c | 18 | QEI rightEncoder(D2, D13, NC, 378); |
apriljunio | 0:99538487421b | 19 | |
apriljunio | 0:99538487421b | 20 | void messageCb( const geometry_msgs::Vector3& msg) |
apriljunio | 0:99538487421b | 21 | { |
apriljunio | 1:5f2cb6bc0a8e | 22 | motorLeft.speed(msg.x); |
apriljunio | 1:5f2cb6bc0a8e | 23 | motorRight.speed(msg.y); |
apriljunio | 0:99538487421b | 24 | } |
apriljunio | 0:99538487421b | 25 | |
apriljunio | 3:9202fa788d3c | 26 | |
apriljunio | 3:9202fa788d3c | 27 | geometry_msgs::Quaternion wheelMsg; |
apriljunio | 3:9202fa788d3c | 28 | ros::Publisher wheelPub("/odom_msg", &wheelMsg); |
apriljunio | 3:9202fa788d3c | 29 | |
apriljunio | 3:9202fa788d3c | 30 | //std_msgs::String str_msg; |
apriljunio | 3:9202fa788d3c | 31 | //ros::Publisher chatter("chatter", &str_msg); |
apriljunio | 3:9202fa788d3c | 32 | |
apriljunio | 0:99538487421b | 33 | ros::Subscriber<geometry_msgs::Vector3> sub("/motor/command", &messageCb ); |
apriljunio | 0:99538487421b | 34 | |
apriljunio | 3:9202fa788d3c | 35 | int leftPulseA=0; |
apriljunio | 3:9202fa788d3c | 36 | int rightPulseA=0; |
apriljunio | 3:9202fa788d3c | 37 | bool changeFlagR=false; |
apriljunio | 3:9202fa788d3c | 38 | bool changeFlagL=false; |
apriljunio | 3:9202fa788d3c | 39 | unsigned long rightTime; |
apriljunio | 3:9202fa788d3c | 40 | unsigned long leftTime; |
apriljunio | 3:9202fa788d3c | 41 | |
apriljunio | 2:82249618b6bc | 42 | |
apriljunio | 0:99538487421b | 43 | int main() { |
apriljunio | 0:99538487421b | 44 | |
apriljunio | 0:99538487421b | 45 | nh.initNode(); |
apriljunio | 0:99538487421b | 46 | nh.subscribe(sub); |
apriljunio | 3:9202fa788d3c | 47 | //nh.advertise(chatter); |
apriljunio | 3:9202fa788d3c | 48 | nh.advertise(wheelPub); |
apriljunio | 0:99538487421b | 49 | |
apriljunio | 0:99538487421b | 50 | while (true) { |
apriljunio | 2:82249618b6bc | 51 | |
apriljunio | 3:9202fa788d3c | 52 | leftPulseA=leftEncoder.getPulses(); |
apriljunio | 3:9202fa788d3c | 53 | rightPulseA = rightEncoder.getPulses(); |
apriljunio | 3:9202fa788d3c | 54 | leftTime = leftEncoder.getTime(); |
apriljunio | 3:9202fa788d3c | 55 | rightTime = rightEncoder.getTime(); |
apriljunio | 3:9202fa788d3c | 56 | |
apriljunio | 2:82249618b6bc | 57 | wheelMsg.x= leftPulseA; |
apriljunio | 2:82249618b6bc | 58 | wheelMsg.y = rightPulseA; |
apriljunio | 3:9202fa788d3c | 59 | wheelMsg.z = leftTime; |
apriljunio | 3:9202fa788d3c | 60 | wheelMsg.w = rightTime; |
apriljunio | 3:9202fa788d3c | 61 | |
apriljunio | 3:9202fa788d3c | 62 | changeFlagL=leftEncoder.getChange(); |
apriljunio | 3:9202fa788d3c | 63 | changeFlagR=rightEncoder.getChange(); |
apriljunio | 2:82249618b6bc | 64 | |
apriljunio | 3:9202fa788d3c | 65 | if((changeFlagL==true)|(changeFlagR==true)){ |
apriljunio | 3:9202fa788d3c | 66 | wheelPub.publish(&wheelMsg); |
apriljunio | 3:9202fa788d3c | 67 | changeFlagL=false; |
apriljunio | 3:9202fa788d3c | 68 | changeFlagR=false; |
apriljunio | 3:9202fa788d3c | 69 | } |
apriljunio | 3:9202fa788d3c | 70 | |
apriljunio | 2:82249618b6bc | 71 | nh.spinOnce(); |
apriljunio | 3:9202fa788d3c | 72 | wait_ms(1000); |
apriljunio | 0:99538487421b | 73 | } |
apriljunio | 0:99538487421b | 74 | } |
apriljunio | 0:99538487421b | 75 |