phidgets_range_sensors.cpp

Go to the documentation of this file.

// see http://www.phidgets.com/products.php?category=2&product_id=3520_0
// distance calculation: Distance (cm) = 2076/(SensorValue - 11)             (the formula is only valid for a SensorValue between 80 to 530)
// TODO: separate into two packages: cob_phidgets_driver (publishing only raw values as sensor_msgs/Range message) and cob_tray_status (evaluating the Range messages to decide if tray is occupied or not, eventually also at which position it is occupied) 

#include "ros/ros.h"
#include "std_msgs/Bool.h"
#include "sensor_msgs/Range.h"

#include <libphidgets/phidget21.h>
#include <sstream>

class Sensor
{
        ros::NodeHandle n_;
        ros::Publisher pub_range_;
        int id_, filter_size_;
        std::list<int> vals_;
        std::string frame_id_;
public:

        Sensor(const std::string &fr_id, const int id, const int filter_size = 10) :
                        id_(id), filter_size_(filter_size), frame_id_(fr_id)
        {
                char buffer[256];
                sprintf(buffer, "range_%d", id);
                pub_range_ = n_.advertise<sensor_msgs::Range>(buffer, 0);
        }

        void publish()
        {
                pub_range_.publish((sensor_msgs::Range) *this);
        }

        operator sensor_msgs::Range() const
        {
                sensor_msgs::Range msg;
                msg.header.stamp = ros::Time::now();
                msg.header.frame_id = frame_id_;
                msg.radiation_type = sensor_msgs::Range::INFRARED;
                msg.min_range = 0.04;
                msg.max_range = 0.3;
                msg.field_of_view = 0; //not given!

                int sum = 0, num = 0;
                for (std::list<int>::const_iterator it = vals_.begin();
                                it != vals_.end(); it++)
                {
                        sum += *it;
                        ++num;
                }
                msg.range = 20.76 / (sum / (double) num - 11.);

                return msg;
        }

        int getId() const
        {
                return id_;
        }

        void update(const int v)
        {
                if ((int)vals_.size() < filter_size_)
                        vals_.push_back(v);
                else
                {
                        vals_.push_back(v);
                        vals_.pop_front();
                }
        }

};

void display_generic_properties(CPhidgetHandle phid)
{
        int sernum, version;
        const char *deviceptr, *label;
        CPhidget_getDeviceType(phid, &deviceptr);
        CPhidget_getSerialNumber(phid, &sernum);
        CPhidget_getDeviceVersion(phid, &version);
        CPhidget_getDeviceLabel(phid, &label);

        ROS_INFO("%s", deviceptr);
        ROS_INFO("Version: %8d SerialNumber: %10d", version, sernum);
        ROS_INFO("Label: %s", label);
        return;
}

int IFK_AttachHandler(CPhidgetHandle IFK, void *userptr)
{
        //CPhidgetInterfaceKit_setSensorChangeTrigger((CPhidgetInterfaceKitHandle)IFK, 0, 0);
        //printf("Attach handler ran!\n");
        return 0;
}

int IFK_SensorChangeHandler(CPhidgetInterfaceKitHandle IFK, void *userptr,
                int Index, int Value)
{
        std::vector<Sensor>* g_sensors = (std::vector<Sensor>*) userptr;
        for (size_t i = 0; i < g_sensors->size(); i++)
                if ((*g_sensors)[i].getId() == Index)
                        (*g_sensors)[i].update(Value);
        return 0;
}

int main(int argc, char **argv)
{
        ros::init(argc, argv, "cob_phidgets");

        ros::NodeHandle nh_("~");
        std::vector<Sensor> g_sensors;
        if (nh_.hasParam("sensors"))
        {
                XmlRpc::XmlRpcValue v;
                nh_.param("sensors", v, v);
                for (int i = 0; i < v.size(); i++)
                {
                        ROS_ASSERT(v[i].size()>=2);

                        int id = v[i][0];
                        std::string fr_id = v[i][1];
                        int filter = v.size() > 2 ? (int) v[i][2] : 10;

                        g_sensors.push_back(Sensor(fr_id, id, filter));
                }
        }
        else
        {
                ROS_ERROR("Parameter sensors not set, shutting down node...");
                nh_.shutdown();
                return false;
        }

        ros::Rate loop_rate(10);

        //init and open phidget
        int numInputs, numOutputs, numSensors;
        int err;

        CPhidgetInterfaceKitHandle IFK = 0;
        CPhidget_enableLogging(PHIDGET_LOG_VERBOSE, NULL);
        CPhidgetInterfaceKit_create(&IFK);
        CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK,
                        IFK_SensorChangeHandler, (void*) &g_sensors);
        CPhidget_set_OnAttach_Handler((CPhidgetHandle) IFK, IFK_AttachHandler,
                        NULL);
        //opening phidget
        CPhidget_open((CPhidgetHandle) IFK, -1);

        //wait 5 seconds for attachment
        ROS_INFO("waiting for phidgets attachement...");
        if ((err = CPhidget_waitForAttachment((CPhidgetHandle) IFK, 10000))
                        != EPHIDGET_OK)
        {
                const char *errStr;
                CPhidget_getErrorDescription(err, &errStr);
                ROS_ERROR("Error waiting for attachment: (%d): %s", err, errStr);
                goto exit;
        }
        ROS_INFO("... attached");

        display_generic_properties((CPhidgetHandle) IFK);
        CPhidgetInterfaceKit_getOutputCount((CPhidgetInterfaceKitHandle) IFK,
                        &numOutputs);
        CPhidgetInterfaceKit_getInputCount((CPhidgetInterfaceKitHandle) IFK,
                        &numInputs);
        CPhidgetInterfaceKit_getSensorCount((CPhidgetInterfaceKitHandle) IFK,
                        &numSensors);
        //CPhidgetInterfaceKit_setOutputState((CPhidgetInterfaceKitHandle)IFK, 0, 1);

        ROS_INFO(
                        "Sensors:%d Inputs:%d Outputs:%d", numSensors, numInputs, numOutputs);

        while (ros::ok())
        {
                for (size_t i = 0; i < g_sensors.size(); i++)
                        g_sensors[i].publish();
                ros::spinOnce();
                loop_rate.sleep();
        }

        exit: CPhidget_close((CPhidgetHandle) IFK);
        CPhidget_delete((CPhidgetHandle) IFK);

        return 0;
}