Merge branch 'OWRS-297-Claw-Numbers'

Conflicts:
	gui/src/owr_gui/include/NavigationGUI.h
	gui/src/owr_gui/include/NavigationNode.h
	gui/src/owr_gui/src/NavigationGUI.cpp
	gui/src/owr_gui/src/NavigationNode.cpp

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

gui/src/owr_gui/include/NavigationNode.h

@@ -12,10 +12,14 @@
 #include <sensor_msgs/NavSatFix.h>
 #include <sensor_msgs/Image.h>
 #include "owr_messages/status.h"
+#include "owr_messages/voltage.h"
+#include "owr_messages/adc.h"
 #include "NavigationGUI.h"
 #include <image_transport/image_transport.h>
+#include <std_msgs/Float32.h>
 #include <std_msgs/Int16.h>
-
+#include <string>
+#include <algorithm> // 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<float> pot_vals;
+		std::vector<std::string> pot_names;
 		double altitude;
 		unsigned char feeds[TOTAL_FEEDS];
 		vector3D target;

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();
+
+}

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);