MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Dependencies:   ros_lib_kinetic

HLComms.h

Committer:
WD40andTape
Date:
2019-02-08
Revision:
29:10a5cf37a875
Parent:
28:8e0c502c1a50
Child:
30:6c8eea90735e

File content as of revision 29:10a5cf37a875:

// HLComms.h

#ifndef HLCOMMS_H
#define HLCOMMS_H

// STANDARD IMPORTS
#include <algorithm>
// MBED IMPORTS
#include"mbed.h"
#include "mbed_events.h"
// ROS IMPORTS
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int16MultiArray.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 speed_mmps;
};

class HLComms
{
    public:
        
        Semaphore newData;
        
        HLComms(unsigned short int freq_hz);
        //~HLComms(void);
        demands_struct get_demands(void);
        void send_text_message(char text[]);
        void send_duration_message(double dblTime);
        void send_sensor_message(unsigned int positions[], unsigned int pressures[]);
    
    private:
    
        unsigned short int _freq_hz;
        struct demands_struct _input;
        
        Thread _threadROS;
        Semaphore _rosReady;
        Semaphore _semSpin;
        ros::NodeHandle _nh;
        ros::Publisher* _text_pub;
        std_msgs::String _text_msg;
        std_msgs::Int16MultiArray _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::Int16MultiArray &demands_array);

};

#endif