diff --git a/gui/src/owr_gui/include/NavigationGUI.h b/gui/src/owr_gui/include/NavigationGUI.h index 18a0263f..b591bdb1 100644 --- a/gui/src/owr_gui/include/NavigationGUI.h +++ b/gui/src/owr_gui/include/NavigationGUI.h @@ -54,14 +54,15 @@ #define ALPHA 1.0 // transparency factor #define TEXTBOX_ALPHA 0.001 - class NavigationGUI : public GLUTWindow { public: NavigationGUI(int width, int height, int *argc, char **argv); void updateInfo(float battery, float signal, float ultrasonic, ListNode current, vector3D target, float lidar); + void updateVoltage(float voltage); void updateVideo(unsigned char *frame, int width, int height); void updateFeedsStatus(unsigned char *feeds, int numOnline); + void updateADC(float effort, float actual, float target); private: // GLUT essential functions @@ -95,10 +96,9 @@ class NavigationGUI : public GLUTWindow { void drawBattery(); void drawSignal(); void drawUltrasonic(); - - + void drawVolts(); + void drawADC(); void drawLidarTilt(); - // pointer to the ROS handler void *navigationNode; @@ -109,6 +109,10 @@ class NavigationGUI : public GLUTWindow { float tiltY; // tilt of forward-back in degrees float ultrasonic; float lidarTilt; + float voltage; // voltage from voltmeter + float actual_claw; + float effort_claw; + float target_claw; vector3D currentPos; double pathRotation; // angle to rotate GPS path when drawing diff --git a/gui/src/owr_gui/include/NavigationNode.h b/gui/src/owr_gui/include/NavigationNode.h index 562cd546..11d4fbd1 100644 --- a/gui/src/owr_gui/include/NavigationNode.h +++ b/gui/src/owr_gui/include/NavigationNode.h @@ -12,10 +12,14 @@ #include #include #include "owr_messages/status.h" +#include "owr_messages/voltage.h" +#include "owr_messages/adc.h" #include "NavigationGUI.h" #include +#include #include - +#include +#include // for finding the right value // The message structs needed for availableFeeds #include "owr_messages/activeCameras.h" #include "owr_messages/stream.h" @@ -30,6 +34,8 @@ class NavigationNode { void receiveGpsMsg(const sensor_msgs::NavSatFix::ConstPtr& msg); void receiveBatteryMsg(const owr_messages::status::ConstPtr& msg); void receiveVideoMsg(const sensor_msgs::Image::ConstPtr& msg); + void receiveVoltageMsg(const std_msgs::Float32::ConstPtr& msg); + void receiveClawMsg(const owr_messages::adc::ConstPtr& msg); void receiveLidarMsg(const std_msgs::Int16::ConstPtr& msg); private: @@ -41,13 +47,18 @@ class NavigationNode { image_transport::Subscriber videoSub[TOTAL_FEEDS]; ros::Subscriber feedsSub; - + ros::Subscriber voltSub; // voltmeter callback + ros::Subscriber adcSub; // adc callback + float battery; float signal; float tiltX; float tiltY; float ultrasonic; float lidar; + float voltage; + std::vector pot_vals; + std::vector pot_names; double altitude; unsigned char feeds[TOTAL_FEEDS]; vector3D target; diff --git a/gui/src/owr_gui/src/NavigationGUI.cpp b/gui/src/owr_gui/src/NavigationGUI.cpp index eac5514a..639c970a 100644 --- a/gui/src/owr_gui/src/NavigationGUI.cpp +++ b/gui/src/owr_gui/src/NavigationGUI.cpp @@ -92,6 +92,16 @@ void NavigationGUI::updateInfo(float bat, float sig, float ultrason, ListNode cu //ROS_INFO("Updated info"); } +void NavigationGUI::updateVoltage(float volts) { + voltage = volts; +} + +void NavigationGUI::updateADC(float effort, float actual, float target) { + actual_claw = actual; + effort_claw = effort; + target_claw = target; +} + void NavigationGUI::updateVideo(unsigned char *frame, int width, int height) { // use the Video_Feed_Frame object method videoScreen->setNewStreamFrame(frame, width, height); @@ -218,6 +228,8 @@ void NavigationGUI::display() { drawBattery(); drawSignal(); drawUltrasonic(); + drawVolts(); + drawADC(); drawLidarTilt(); } @@ -651,3 +663,39 @@ void NavigationGUI::special_keyup(int keycode, int x, int y) { void NavigationGUI::keyup(unsigned char key, int x, int y) { } + +// draw Voltage +void NavigationGUI::drawVolts() { + glPushMatrix(); + + char text[30]; + + glTranslated(currWinW/2.0-30,-50, 0); + glColor4f(1, 0, 0, ALPHA); + sprintf(text, "%.2fV", voltage); + drawText(text, GLUT_BITMAP_TIMES_ROMAN_24, 0, 0); + + glPopMatrix(); +} + + +void NavigationGUI::drawADC() { + + glPushMatrix(); + glTranslated(currWinW - currWinW/10.0, -currWinH/2.0, 0); + glColor4f(0, 1, 0, TEXTBOX_ALPHA); + glBegin(GL_QUADS); + glVertex2d(-10, 24); + glVertex2d(-10, -75); + glVertex2d(200, -75); + glVertex2d(200, 24); + glEnd(); + + char text[60]; + glColor4f(1, 0, 0, ALPHA); + sprintf(text, "Target: %.2f \nActual: %.2f \nEffort: %.2f",target_claw, actual_claw, effort_claw); + drawText(text, GLUT_BITMAP_TIMES_ROMAN_24, 0, 0); + + glPopMatrix(); + +} \ No newline at end of file diff --git a/gui/src/owr_gui/src/NavigationNode.cpp b/gui/src/owr_gui/src/NavigationNode.cpp index ef717298..5502b241 100644 --- a/gui/src/owr_gui/src/NavigationNode.cpp +++ b/gui/src/owr_gui/src/NavigationNode.cpp @@ -42,10 +42,10 @@ NavigationNode::NavigationNode(NavigationGUI *newgui) { gpsSub = n.subscribe("/gps/fix", 1000, &NavigationNode::receiveGpsMsg, this); // GPS related data batterySub = n.subscribe("/status/battery", 1000, &NavigationNode::receiveBatteryMsg, this); // Power left on the battery feedsSub = n.subscribe("/owr/control/availableFeeds", 1000, &NavigationNode::receiveFeedsStatus, this); - + voltSub = n.subscribe("/owr/voltmeter", 1000, &NavigationNode::receiveVoltageMsg, this); + adcSub = n.subscribe("/owr/adc", 1000, &NavigationNode::receiveClawMsg, this); lidarModeSub = n.subscribe("/owr/lidar_gimble_mode", 1000, &NavigationNode::receiveLidarMsg, this); - // Subscribe to all topics that will be published to by cameras, if the topic hasnt been // created yet, will wait til it has w/o doing anything @@ -135,6 +135,45 @@ void NavigationNode::receiveVideoMsg(const sensor_msgs::Image::ConstPtr& msg) { gui->updateVideo((unsigned char *)msg->data.data(), msg->width, msg->height); } +void NavigationNode::receiveVoltageMsg(const std_msgs::Float32::ConstPtr& msg) { + assert(msg); // + + //ROS_INFO("received a message"); + //ROS_INFO("voltage %f", msg->voltage); + voltage = msg->data; + gui->updateVoltage(voltage); + ROS_INFO("voltage %f", voltage); +} + +void NavigationNode::receiveClawMsg(const owr_messages::adc::ConstPtr& msg) { + + assert(msg); + pot_vals = msg->pot; + pot_names = msg->potFrame; + + int effort_index; + int target_index; + int actual_index; + + effort_index = find(pot_names.begin(), pot_names.end(), "clawEffort") - pot_names.begin(); + target_index = find(pot_names.begin(), pot_names.end(), "clawGrip") - pot_names.begin(); + actual_index = find(pot_names.begin(), pot_names.end(), "clawActual") - pot_names.begin(); + + float effort = pot_vals[effort_index]; + float actual = pot_vals[actual_index]; + float target = pot_vals[target_index]; + + ROS_INFO("EFFORT: %f", effort); + ROS_INFO("ACTUAL: %f", actual); + ROS_INFO("TARGET: %f", target); + + ROS_INFO("EFFORT INDEX: %d", effort_index); + ROS_INFO("ACTUAL INDEX: %d", actual_index); + ROS_INFO("TARGET INDEX: %d", target_index); + + gui->updateADC(effort, actual, target); + +} void NavigationNode::receiveLidarMsg(const std_msgs::Int16::ConstPtr& msg) { assert(msg);