SBD Digital Accelerator Robotics / Mbed 2 deprecated Mouse_ISR_test1

added broadcaster for l.mouse

Dependencies:   QEI mbed ros_lib_kinetic

Fork of Mouse_ISR_broadcaster by SBD Digital Accelerator Robotics

main.cpp@5:e674692f6d15, 2018-07-23 (annotated)

Committer:
apriljunio
Date:
Mon Jul 23 14:38:21 2018 +0000
Revision:
5:e674692f6d15
Parent:
4:f87a415beec4 
Trying to broadcast Mouse odometry

Who changed what in which revision?

UserRevisionLine numberNew contents of line
apriljunio 0:1a95e3b1026a 1 #include "mbed.h"
apriljunio 0:1a95e3b1026a 2 #include "QEI.h"
apriljunio 0:1a95e3b1026a 3 #include "ros.h"
apriljunio 0:1a95e3b1026a 4 #include "geometry_msgs/Quaternion.h"
apriljunio 4:f87a415beec4 5 #include <tf/transform_broadcaster.h>
apriljunio 4:f87a415beec4 6 #include <nav_msgs/Odometry.h>
apriljunio 0:1a95e3b1026a 7
apriljunio 0:1a95e3b1026a 8 //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
apriljunio 3:ab392a9f941d 9 QEI leftX(D4, D5, NC, 378,QEI::X2_ENCODING);
apriljunio 3:ab392a9f941d 10 QEI leftY(D10, D11, NC, 378,QEI::X2_ENCODING);
apriljunio 2:47663f3fec3a 11 QEI rightX(D0, D1, NC, 378,QEI::X2_ENCODING);
apriljunio 2:47663f3fec3a 12 QEI rightY(D2, D3, NC, 378,QEI::X2_ENCODING);
apriljunio 0:1a95e3b1026a 13
apriljunio 0:1a95e3b1026a 14 ros::NodeHandle nh;
apriljunio 5:e674692f6d15 15 geometry_msgs::Quaternion wheelMsg;
apriljunio 5:e674692f6d15 16
apriljunio 4:f87a415beec4 17
apriljunio 4:f87a415beec4 18 geometry_msgs::Quaternion odom_quat;
apriljunio 4:f87a415beec4 19 geometry_msgs::TransformStamped odom_trans;
apriljunio 4:f87a415beec4 20 tf::TransformBroadcaster odom_broadcaster;
apriljunio 5:e674692f6d15 21 ros::Publisher wheelPub("/odom_msg", &wheelMsg);
apriljunio 0:1a95e3b1026a 22
apriljunio 1:7ba9803d4e01 23 //rosserial does not like volatile ints
apriljunio 2:47663f3fec3a 24 int leftPosX=0;
apriljunio 2:47663f3fec3a 25 int leftPosY=0;
apriljunio 2:47663f3fec3a 26 int rightPosX=0;
apriljunio 2:47663f3fec3a 27 int rightPosY=0;
apriljunio 0:1a95e3b1026a 28
apriljunio 4:f87a415beec4 29 double theta=0;
apriljunio 4:f87a415beec4 30 ros::Time current_time;
apriljunio 4:f87a415beec4 31
apriljunio 0:1a95e3b1026a 32 int main(){
apriljunio 0:1a95e3b1026a 33 nh.initNode();
apriljunio 5:e674692f6d15 34 odom_broadcaster.init(nh);
apriljunio 0:1a95e3b1026a 35 //nh.advertise(chatter);
apriljunio 5:e674692f6d15 36 nh.advertise(wheelPub);
apriljunio 5:e674692f6d15 37 //nh.advertise(odom_quat);
apriljunio 2:47663f3fec3a 38 // pc.baud(57600);
apriljunio 0:1a95e3b1026a 39
apriljunio 4:f87a415beec4 40
apriljunio 0:1a95e3b1026a 41 while(true){
apriljunio 0:1a95e3b1026a 42
apriljunio 4:f87a415beec4 43 current_time = nh.now();
apriljunio 4:f87a415beec4 44
apriljunio 2:47663f3fec3a 45 leftPosX = leftX.getPulses();
apriljunio 2:47663f3fec3a 46 leftPosY = leftY.getPulses();
apriljunio 2:47663f3fec3a 47 rightPosX = rightX.getPulses();
apriljunio 2:47663f3fec3a 48 rightPosY = rightY.getPulses();
apriljunio 0:1a95e3b1026a 49
apriljunio 5:e674692f6d15 50 wheelMsg.x= leftPosX;
apriljunio 2:47663f3fec3a 51 wheelMsg.y = leftPosY;
apriljunio 2:47663f3fec3a 52 wheelMsg.z = rightPosX;
apriljunio 5:e674692f6d15 53 wheelMsg.w = rightPosY;
apriljunio 4:f87a415beec4 54
apriljunio 4:f87a415beec4 55 odom_quat.x=0;
apriljunio 4:f87a415beec4 56 odom_quat.y=0;
apriljunio 4:f87a415beec4 57 odom_quat.z=0;
apriljunio 4:f87a415beec4 58 odom_quat.w=0;
apriljunio 4:f87a415beec4 59
apriljunio 0:1a95e3b1026a 60
apriljunio 4:f87a415beec4 61 odom_trans.header.stamp= current_time;
apriljunio 5:e674692f6d15 62 odom_trans.header.frame_id="world";
apriljunio 4:f87a415beec4 63 odom_trans.header.stamp = current_time;
apriljunio 4:f87a415beec4 64 odom_trans.child_frame_id = "base_link";
apriljunio 4:f87a415beec4 65 odom_trans.transform.translation.x = rightPosX;
apriljunio 4:f87a415beec4 66 odom_trans.transform.translation.y = rightPosY;
apriljunio 4:f87a415beec4 67 odom_trans.transform.translation.z = 0.0;
apriljunio 4:f87a415beec4 68 odom_trans.transform.rotation = odom_quat;
apriljunio 4:f87a415beec4 69
apriljunio 4:f87a415beec4 70 //send the transform
apriljunio 4:f87a415beec4 71 odom_broadcaster.sendTransform(odom_trans);
apriljunio 4:f87a415beec4 72
apriljunio 5:e674692f6d15 73 wheelPub.publish(&wheelMsg);
apriljunio 0:1a95e3b1026a 74 nh.spinOnce();
apriljunio 5:e674692f6d15 75 wait(1000);
apriljunio 0:1a95e3b1026a 76 }
apriljunio 0:1a95e3b1026a 77 }