Integration of multiple sensors with ROS Serial

Dependencies:   mbed

main.cpp

Committer:
zillkhan
Date:
2021-10-23
Revision:
0:d409714cebbf

File content as of revision 0:d409714cebbf:

#include <ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include "SHARPIR.h"


ros::NodeHandle  nh;

std_msgs::Float32 data;
ros::Publisher sharpir("sharpir", &data);

SHARPIR Sensor(A0); 


int main() {
    float DistanceCM;
    
    nh.initNode();
    nh.advertise(sharpir);
    
    while (1) { //creates an eternal loop
    
        DistanceCM=Sensor.cm();  
        //sprintf (buffer, "%f", DistanceCM);
        
        data.data = DistanceCM;
        sharpir.publish( &data );
        
        nh.spinOnce();
        wait_ms(1000);
    }
}