MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Dependencies:   ros_lib_kinetic

HLComms.h

Committer:
WD40andTape
Date:
2019-02-06
Revision:
28:8e0c502c1a50
Parent:
19:e5acb2183d4e
Child:
29:10a5cf37a875

File content as of revision 28:8e0c502c1a50:

// HLComms.h

#ifndef HLCOMMS_H
#define HLCOMMS_H

// STANDARD IMPORTS
// MBED IMPORTS
#include"mbed.h"
#include "mbed_events.h"
// ROS IMPORTS
#include <ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
// CUSTOM IMPORTS
#include "MLSettings.h"

struct demands_struct {
    //double psi_mm[N_CHANNELS];
    double psi_mm[9];
    double speed_mmps;
};

class HLComms
{
    public:
        
        Semaphore newData;
        
        HLComms(unsigned short int freq_hz);
        //~HLComms(void);
        demands_struct get_demands(void);
        void send_duration_message(double dblTime);
        void send_sensor_message(double positions[], double pressures[]);
    
    private:
    
        unsigned short int _freq_hz;
        struct demands_struct _input;
        
        Thread _threadROS;
        Semaphore _rosReady;
        Semaphore _semSpin;
        ros::NodeHandle _nh;
        //ros::Subscriber<std_msgs::Float32MultiArray>* _demands_sub;
        std_msgs::Float32MultiArray _sensor_msg;
        ros::Publisher* _sensor_pub;
        std_msgs::Float32 _duration_msg;
        ros::Publisher* _duration_pub;

        Mutex recv_mutex;
        
        void ros_main(void);
        void HLComms::releaseSemSpin(void);
        void receive_demands(const std_msgs::Float32MultiArray &demands_array);

};

#endif