Mid level control code
Dependencies: ros_lib_kinetic
HLComms.cpp
00001 // HLComms.cpp 00002 00003 #include "HLComms.h" 00004 00005 HLComms::HLComms(unsigned short int freq_hz) : // Constructor 00006 newData(1), 00007 _freq_hz(freq_hz), 00008 _threadROS(osPriorityBelowNormal), 00009 _rosReady(1), 00010 _semSpin(1) 00011 { 00012 _threadROS.start(callback(this, &HLComms::ros_main));// Start ROS thread 00013 _rosReady.wait(); 00014 } 00015 00016 /*HLComms::~HLComms(void) { // Destructor 00017 delete *_text_pub; 00018 delete *_sensor_pub; 00019 delete *_durations_pub; 00020 }*/ 00021 00022 // ROS 00023 00024 void HLComms::ros_main(void) { 00025 _nh.getHardware()->setBaud(BAUD_RATE); 00026 _nh.initNode(); 00027 wait_ms(1000); 00028 00029 //ros::Subscriber<std_msgs::Int16MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this); 00030 ros::Subscriber<std_msgs::Float32MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this); 00031 _nh.subscribe(demands_sub); 00032 00033 _text_pub = new ros::Publisher("ML_TextOut", &_text_msg); 00034 _nh.advertise(*_text_pub); 00035 00036 _sensor_msg.data_length = N_CHANNELS*2; 00037 _sensor_pub = new ros::Publisher("ML_Sensors", &_sensor_msg); 00038 _nh.advertise(*_sensor_pub); 00039 00040 _durations_msg.data_length = 3; 00041 _durations_pub = new ros::Publisher("ML_Durations", &_durations_msg); 00042 _nh.advertise(*_durations_pub); 00043 00044 wait_ms(1000); 00045 _rosReady.release(); 00046 00047 Ticker spinTicker; 00048 spinTicker.attach(callback(this,&HLComms::releaseSemSpin), 1/(float)_freq_hz); // Set up HL comms to recur at fixed intervals 00049 00050 while (1) { 00051 _semSpin.wait(); 00052 _nh.spinOnce(); 00053 } 00054 } 00055 00056 void HLComms::releaseSemSpin(void) { 00057 _semSpin.release(); 00058 } 00059 00060 // INPUT 00061 00062 void HLComms::receive_demands(const std_msgs::Float32MultiArray &demands_array) { 00063 struct demands_struct _protected_input; 00064 recv_mutex.lock(); 00065 for(int i=0; i<N_CHANNELS; i++) { 00066 _protected_input.psi_mm[i] = (double)demands_array.data[i]; 00067 //_protected_input.psi_mm[i] = min( max(_protected_input.psi_mm[i],0.0) , (double)MAX_ACTUATOR_LIMIT_MM ); 00068 } 00069 _protected_input.speeds_mmps[0] = (double)demands_array.data[9]; 00070 _protected_input.speeds_mmps[0] = min( max(_protected_input.speeds_mmps[0],0.0) , (double)MAX_SPEED_MMPS ); 00071 _protected_input.speeds_mmps[1] = (double)demands_array.data[10]; 00072 _protected_input.speeds_mmps[1] = min( max(_protected_input.speeds_mmps[1],0.0) , (double)MAX_SPEED_MMPS ); 00073 _protected_input.speeds_mmps[2] = (double)demands_array.data[11]; 00074 _protected_input.speeds_mmps[2] = min( max(_protected_input.speeds_mmps[2],0.0) , (double)MAX_SPEED_MMPS ); 00075 _protected_input.utitilies_bool[0] = fabs(demands_array.data[12]) > numeric_limits<float>::epsilon(); 00076 _protected_input.utitilies_bool[1] = fabs(demands_array.data[13]) > numeric_limits<float>::epsilon(); 00077 _protected_input.utitilies_bool[2] = fabs(demands_array.data[14]) > numeric_limits<float>::epsilon(); 00078 _protected_input.utitilies_bool[3] = fabs(demands_array.data[15]) > numeric_limits<float>::epsilon(); 00079 00080 recv_mutex.unlock(); 00081 _input = _protected_input; 00082 newData.release(); 00083 } 00084 00085 demands_struct HLComms::get_demands(void) { 00086 return _input; 00087 } 00088 00089 // OUTPUT 00090 00091 void HLComms::send_text_message(char text[]) { 00092 _text_msg.data = text; 00093 _text_pub->publish(&_text_msg); 00094 } 00095 00096 void HLComms::send_durations_message(double dblTimes[]) { 00097 for(short int i=0; i<3; i++) { 00098 _durations_msg.data[i] = dblTimes[i]; 00099 } 00100 _durations_pub->publish(&_durations_msg); 00101 } 00102 00103 void HLComms::send_sensor_message(unsigned int positions[], unsigned int pressures[]) { 00104 short int i_channel; 00105 for(short int i=0; i<N_CHANNELS*2; i++) { 00106 i_channel = i%N_CHANNELS; 00107 if(i<N_CHANNELS) { 00108 _sensor_msg.data[i] = positions[i_channel]; 00109 } else { 00110 _sensor_msg.data[i] = pressures[i_channel]; 00111 } 00112 } 00113 _sensor_pub->publish(&_sensor_msg); 00114 }
Generated on Mon Jul 25 2022 18:11:35 by
1.7.2