added broadcaster for l.mouse
Dependencies: QEI mbed ros_lib_kinetic
Fork of Mouse_ISR_broadcaster by
Diff: main.cpp
- Revision:
- 7:f71761cac14a
- Parent:
- 6:1508faf1f383
- Child:
- 8:f118cc66e271
--- a/main.cpp Fri Jul 20 18:41:20 2018 +0000 +++ b/main.cpp Mon Jul 23 20:56:32 2018 +0000 @@ -1,10 +1,22 @@ #include "mbed.h" #include "QEI.h" #include "ros.h" +#include "ros/time.h" +#include <tf/tf.h> #include "geometry_msgs/Quaternion.h" #include <tf/transform_broadcaster.h> #include <nav_msgs/Odometry.h> +#include "geometry_msgs/Vector3.h" +#define ODOM_TICK_TOPIC "/odom_msg" +#define ODOM_TOPIC "/odom" + +#define ODOM_FRAME "odom" +#define BASE_FRAME "base_link" + +#define RADTODEG(angleInRadians) ((angleInRadians) * 180.0 / M_PI) + +ros::NodeHandle nh; //PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT) QEI leftX(D4, D5, NC, 378,QEI::X2_ENCODING); @@ -12,39 +24,65 @@ 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; -geometry_msgs::Quaternion odom_quat; ros::Publisher wheelPub("/odom_msg", &wheelMsg); +//for odom_pub +//geometry_msgs::Quaternion odom_quat; geometry_msgs::TransformStamped odom_trans; +struct OdomValues { + double x; + double y; + double theta; + + OdomValues(double x, double y, double theta) { + this->x = x; + this->y = y; + this->theta = theta; + } +}; +nav_msgs::Odometry odomMsg; +ros::Publisher odomPub("odom", &odomMsg); +geometry_msgs::TransformStamped odomTransform; +tf::TransformBroadcaster odomBroadcaster; + +OdomValues currentOdom(0.0, 0.0, 0.0); + +void publishOdometry(); + + + //rosserial does not like volatile ints int leftPosX=0; int leftPosY=0; int rightPosX=0; int rightPosY=0; -double theta=0; -ros::Time current_time; +//double theta=0; +//ros::Time current_time; int main(){ nh.initNode(); - //nh.advertise(chatter); nh.advertise(wheelPub); + + nh.advertise(odomPub); + odomBroadcaster.init(nh); // pc.baud(57600); - tf::TransformBroadcaster odom_broadcaster; while(true){ - current_time = ros::Time::now(); leftPosX = leftX.getPulses(); leftPosY = leftY.getPulses(); rightPosX = rightX.getPulses(); rightPosY = rightY.getPulses(); - wheelMsg.x= leftPosX; - wheelMsg.y = leftPosY; - wheelMsg.z = rightPosX; - wheelMsg.w = rightPosY; + wheelMsg.x= leftPosX/10; + wheelMsg.y = leftPosY/10; + wheelMsg.z = rightPosX/10; + wheelMsg.w = rightPosY/10; wheelPub.publish(&wheelMsg); + + +/* current_time = nh.now(); odom_quat.x=0; odom_quat.y=0; @@ -64,7 +102,35 @@ //send the transform odom_broadcaster.sendTransform(odom_trans); + */ + publishOdometry(); nh.spinOnce(); - wait(0.01); + wait(.1); } -} \ No newline at end of file +} +void publishOdometry() { + // Set initial header values + odomTransform.header.stamp = nh.now(); + odomTransform.header.frame_id = ODOM_FRAME; + odomTransform.child_frame_id = BASE_FRAME; + + odomMsg.header.stamp = odomTransform.header.stamp; + odomMsg.header.frame_id = odomTransform.header.frame_id; + odomMsg.child_frame_id = odomTransform.child_frame_id; + + geometry_msgs::Quaternion rotation = tf::createQuaternionFromYaw(currentOdom.theta); + + odomTransform.transform.translation.x = wheelMsg.x; + odomTransform.transform.translation.y = wheelMsg.y; + odomTransform.transform.translation.z = 0.0; + odomTransform.transform.rotation = rotation; + + odomMsg.pose.pose.position.x = wheelMsg.x; + odomMsg.pose.pose.position.y = wheelMsg.y; + odomMsg.pose.pose.position.z = 0.0; + odomMsg.pose.pose.orientation = rotation; + + // Publish result + odomPub.publish(&odomMsg); + odomBroadcaster.sendTransform(odomTransform); +}