MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Dependencies:   ros_lib_kinetic

HLComms.cpp@28:8e0c502c1a50, 2019-02-06 (annotated)

Committer:
WD40andTape
Date:
Wed Feb 06 16:10:18 2019 +0000
Revision:
28:8e0c502c1a50
Parent:
19:e5acb2183d4e
Child:
29:10a5cf37a875
Tested and working (separate from HL).

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WD40andTape 7:5b6a2cefbf3b 1 // HLComms.cpp
WD40andTape 7:5b6a2cefbf3b 2
WD40andTape 7:5b6a2cefbf3b 3 #include "HLComms.h"
WD40andTape 7:5b6a2cefbf3b 4
WD40andTape 28:8e0c502c1a50 5 HLComms::HLComms(unsigned short int freq_hz) : // Constructor
WD40andTape 28:8e0c502c1a50 6 newData(1),
WD40andTape 28:8e0c502c1a50 7 _freq_hz(freq_hz),
WD40andTape 28:8e0c502c1a50 8 _threadROS(osPriorityBelowNormal),
WD40andTape 28:8e0c502c1a50 9 _rosReady(1),
WD40andTape 28:8e0c502c1a50 10 _semSpin(1)
WD40andTape 28:8e0c502c1a50 11 {
WD40andTape 28:8e0c502c1a50 12 _threadROS.start(callback(this, &HLComms::ros_main));// Start ROS thread
WD40andTape 28:8e0c502c1a50 13 _rosReady.wait();
WD40andTape 7:5b6a2cefbf3b 14 }
WD40andTape 7:5b6a2cefbf3b 15
WD40andTape 28:8e0c502c1a50 16 /*HLComms::~HLComms(void) { // Destructor
WD40andTape 28:8e0c502c1a50 17 delete *_sensor_pub;
WD40andTape 28:8e0c502c1a50 18 delete *_duration_pub;
WD40andTape 28:8e0c502c1a50 19 }*/
WD40andTape 28:8e0c502c1a50 20
WD40andTape 28:8e0c502c1a50 21 // ROS
WD40andTape 28:8e0c502c1a50 22
WD40andTape 28:8e0c502c1a50 23 void HLComms::ros_main(void) {
WD40andTape 28:8e0c502c1a50 24 //printf("HELLO\r\n");
WD40andTape 28:8e0c502c1a50 25 _nh.getHardware()->setBaud(BAUD_RATE);
WD40andTape 28:8e0c502c1a50 26 _nh.initNode();
WD40andTape 28:8e0c502c1a50 27 wait_ms(1000);
WD40andTape 28:8e0c502c1a50 28
WD40andTape 28:8e0c502c1a50 29 ros::Subscriber<std_msgs::Float32MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
WD40andTape 28:8e0c502c1a50 30 _nh.subscribe(demands_sub);
WD40andTape 28:8e0c502c1a50 31
WD40andTape 28:8e0c502c1a50 32 _sensor_msg.data_length = N_CHANNELS*2;
WD40andTape 28:8e0c502c1a50 33 _sensor_pub = new ros::Publisher("ML_Sensors", &_sensor_msg);
WD40andTape 28:8e0c502c1a50 34 _nh.advertise(*_sensor_pub);
WD40andTape 28:8e0c502c1a50 35
WD40andTape 28:8e0c502c1a50 36 _duration_pub = new ros::Publisher("ML_Duration", &_duration_msg);
WD40andTape 28:8e0c502c1a50 37 _nh.advertise(*_duration_pub);
WD40andTape 28:8e0c502c1a50 38
WD40andTape 28:8e0c502c1a50 39 wait_ms(1000);
WD40andTape 28:8e0c502c1a50 40 _rosReady.release();
WD40andTape 28:8e0c502c1a50 41
WD40andTape 28:8e0c502c1a50 42 Ticker spinTicker;
WD40andTape 28:8e0c502c1a50 43 spinTicker.attach(callback(this,&HLComms::releaseSemSpin), 1/(float)_freq_hz); // Set up HL comms to recur at fixed intervals
WD40andTape 28:8e0c502c1a50 44
WD40andTape 28:8e0c502c1a50 45 while (1) {
WD40andTape 28:8e0c502c1a50 46 _semSpin.wait();
WD40andTape 28:8e0c502c1a50 47 _nh.spinOnce();
WD40andTape 7:5b6a2cefbf3b 48 }
WD40andTape 7:5b6a2cefbf3b 49 }
WD40andTape 7:5b6a2cefbf3b 50
WD40andTape 28:8e0c502c1a50 51 void HLComms::releaseSemSpin(void) {
WD40andTape 28:8e0c502c1a50 52 _semSpin.release();
WD40andTape 9:cd3607ba5643 53 }
WD40andTape 9:cd3607ba5643 54
WD40andTape 28:8e0c502c1a50 55 // INPUT
WD40andTape 9:cd3607ba5643 56
WD40andTape 28:8e0c502c1a50 57 void HLComms::receive_demands(const std_msgs::Float32MultiArray &demands_array) {
WD40andTape 28:8e0c502c1a50 58 struct demands_struct _protected_input;
WD40andTape 28:8e0c502c1a50 59 recv_mutex.lock();
WD40andTape 28:8e0c502c1a50 60 /*for(int i=0; i<N_CHANNELS-1; i++) {
WD40andTape 28:8e0c502c1a50 61 _protected_input.psi_mm[i] = demands_array.data[i];
WD40andTape 7:5b6a2cefbf3b 62 }
WD40andTape 28:8e0c502c1a50 63 short int demands_array_length = sizeof(demands_array.data)/sizeof(demands_array.data[0]);
WD40andTape 28:8e0c502c1a50 64 _protected_input.speed_mmps = demands_array.data[demands_array_length-1];*/
WD40andTape 28:8e0c502c1a50 65 _protected_input.psi_mm[0] = demands_array.data[0];
WD40andTape 28:8e0c502c1a50 66 _protected_input.psi_mm[1] = demands_array.data[1];
WD40andTape 28:8e0c502c1a50 67 _protected_input.psi_mm[2] = demands_array.data[2];
WD40andTape 28:8e0c502c1a50 68 _protected_input.psi_mm[3] = demands_array.data[3];
WD40andTape 28:8e0c502c1a50 69 _protected_input.psi_mm[4] = demands_array.data[4];
WD40andTape 28:8e0c502c1a50 70 _protected_input.psi_mm[5] = demands_array.data[5];
WD40andTape 28:8e0c502c1a50 71 _protected_input.psi_mm[6] = demands_array.data[6];
WD40andTape 28:8e0c502c1a50 72 _protected_input.psi_mm[7] = demands_array.data[7];
WD40andTape 28:8e0c502c1a50 73 _protected_input.psi_mm[8] = demands_array.data[8];
WD40andTape 28:8e0c502c1a50 74 _protected_input.speed_mmps = demands_array.data[9];
WD40andTape 28:8e0c502c1a50 75 recv_mutex.unlock();
WD40andTape 28:8e0c502c1a50 76 _input = _protected_input;
WD40andTape 28:8e0c502c1a50 77 newData.release();
WD40andTape 17:bbaf3e8440ad 78 }
WD40andTape 17:bbaf3e8440ad 79
WD40andTape 28:8e0c502c1a50 80 demands_struct HLComms::get_demands(void) {
WD40andTape 28:8e0c502c1a50 81 return _input;
WD40andTape 28:8e0c502c1a50 82 }
WD40andTape 28:8e0c502c1a50 83
WD40andTape 28:8e0c502c1a50 84 // OUTPUT
WD40andTape 28:8e0c502c1a50 85
WD40andTape 28:8e0c502c1a50 86 void HLComms::send_duration_message(double dblTime) {
WD40andTape 28:8e0c502c1a50 87 _duration_msg.data = dblTime;
WD40andTape 28:8e0c502c1a50 88 _duration_pub->publish(&_duration_msg);
WD40andTape 9:cd3607ba5643 89 }
WD40andTape 9:cd3607ba5643 90
WD40andTape 28:8e0c502c1a50 91 void HLComms::send_sensor_message(double positions[], double pressures[]) {
WD40andTape 28:8e0c502c1a50 92 short int i_channel;
WD40andTape 28:8e0c502c1a50 93 for(short int i=0; i<N_CHANNELS*2; i++) {
WD40andTape 28:8e0c502c1a50 94 i_channel = i%N_CHANNELS;
WD40andTape 28:8e0c502c1a50 95 if(i<N_CHANNELS) {
WD40andTape 28:8e0c502c1a50 96 _sensor_msg.data[i] = positions[i_channel];
WD40andTape 28:8e0c502c1a50 97 } else {
WD40andTape 28:8e0c502c1a50 98 _sensor_msg.data[i] = pressures[i_channel];
WD40andTape 28:8e0c502c1a50 99 }
WD40andTape 28:8e0c502c1a50 100 }
WD40andTape 28:8e0c502c1a50 101 _sensor_pub->publish(&_sensor_msg);
WD40andTape 9:cd3607ba5643 102 }