phidget_reader.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <math.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
#include <string>
#include <phidget_ik/phidget_ik.h>
#include <Grijper/command.h>

using namespace std;

void shiftBuffer(int);
int mean();
void printbuffer();
void shutdown(const std_msgs::Bool::ConstPtr&);
float sensorToDistance(int);

const int sensor_id = 4; 
const int freq = 250; 
const int buffer_size = 25; 
int buffer[buffer_size];  
int main(int argc, char **argv){
        
        ros::init(argc, argv, "phidget_reader");
        ros::NodeHandle n;
        ros::Publisher publisher = n.advertise<std_msgs::Float32>("phidget_value", 1000);
        ros::Subscriber sd = n.subscribe("shutdown", 1, shutdown);
        ros::Rate loop_rate(freq);
        PhidgetIK phidget_ik;
  
        phidget_ik.init(-1);
        phidget_ik.waitForAttachment(1000);

        if (phidget_ik.getLastError()){
                std::cerr << "Error initializing PhidgetInterfaceKit: " << phidget_ik.getErrorDescription(phidget_ik.getLastError()) << std::endl;
                return 1;
        }

        for(int i = 0; i < buffer_size; i++)
        {
                buffer[i] = 1;
        }

        while(ros::ok()){
                int data = phidget_ik.getSensorValue(sensor_id);
                std_msgs::Float32 sensor_val;
                shiftBuffer(data);
                data = mean();

                if(data > 700){
                        printf("Ignoring sensor value of %i\n", data);
                }else{
                        float distance;
                        if(data < 100){
                                distance = 0;
                        } else {
                                distance = sensorToDistance(data);
                        }
                        sensor_val.data = distance;
                        printf("Reading sensor %i, value: %i, distance:%f\n",sensor_id, data, distance);
                        //printbuffer();

                        publisher.publish(sensor_val);
                }
                ros::spinOnce();
                loop_rate.sleep();
        }

        return 0;
}

void shiftBuffer(int data){
        for(int i = 0; i < buffer_size - 1; i++){
                buffer[i] = buffer[i+1];
        }
        buffer[buffer_size - 1] = data;
}

int mean(){
        int size = 0;
        for(int i = 0; i < buffer_size; i
                ++){
                size = size + buffer[i];
        }
        return(ceil(size/buffer_size));
}

void printbuffer(){
        cout << " - [";
        for(int i = 0; i < buffer_size; i++){
                printf("%i,", buffer[i]);
        }
        cout << "]\n";
}

float sensorToDistance(int sensorValue){

        return 2076.0f / (sensorValue - 11.0f);

}

void shutdown(const std_msgs::Bool::ConstPtr& b){
        ros::shutdown();
}