From 8f4bcbb42c100b6a5cd2790cd2dc1a60c9ec3c3d Mon Sep 17 00:00:00 2001
From: Rescue Group <rescue@rescuelap2.(none)>
Date: Thu, 27 Jun 2013 11:57:58 +0200
Subject: [PATCH] MM: Added joint zeroing

---
 crosbot_ui/src/joystick.cpp                  |  1 -
 crosbot_ui/src/renders/robot/joystick.cpp    | 17 ++++-
 crosbot_ui/src/renders/robot/robotwidget.cpp | 69 +++++++++++++++++++-
 3 files changed, 82 insertions(+), 5 deletions(-)

diff --git a/crosbot_ui/src/joystick.cpp b/crosbot_ui/src/joystick.cpp
index e359360..581caa9 100644
--- a/crosbot_ui/src/joystick.cpp
+++ b/crosbot_ui/src/joystick.cpp
@@ -234,7 +234,6 @@ void Joystick::run() {
 
 				char mapping[ABS_CNT];
 				ioctl(device, JSIOCGAXMAP, mapping);
-				LOG("Joystick: Mapping \"%s\"\n", mapping);
 
 				fcntl(device, F_SETFL, O_NONBLOCK);
 
diff --git a/crosbot_ui/src/renders/robot/joystick.cpp b/crosbot_ui/src/renders/robot/joystick.cpp
index 7070832..88b9915 100644
--- a/crosbot_ui/src/renders/robot/joystick.cpp
+++ b/crosbot_ui/src/renders/robot/joystick.cpp
@@ -21,6 +21,20 @@ JoystickRender::JoystickRender(RobotPanel& panel, ConfigElementPtr config) :
 		activeColour(1.0, 0, 0)
 {
 	joystick.configure(config);
+	for (uint32_t i = 0; i < config->getChildCount(); ++i) {
+		ConfigElementPtr child = config->getChild(i);
+
+		if (child != NULL && (strcasecmp(child->name.c_str(), "button") == 0)) {
+			int id =  child->getParamAsInt("id", Joystick::Undefined);
+			id =  child->getParamAsInt("button", id);
+			if (id >= 0 && id < MAX_JOYSTICK_BUTTONS) {
+				std::string key = child->getParam("key");
+				if (key != "") {
+					joystick.buttonKeys[id] = Panel::getKeyForChar(key[0]);
+				}
+			}
+		}
+	}
 }
 
 void JoystickRender::start() {
@@ -63,14 +77,13 @@ void JoystickRender::RenderJoystick::buttonPressed(int b) {
 		turn  *= (0.5 - getRelativePosition(Joystick::Y)) * 2;
 		render.panel.setCurrentSpeeds(speed, turn);
 	} else if (buttonKeys[b] != Joystick::Undefined) {
+		// Options to turn button presses into KeyEvents
 		QKeyEvent keyEvent(QEvent::KeyPress, buttonKeys[b], 0);
 		RobotWidget* widget = dynamic_cast < RobotWidget* > (render.panel.getWidget());
 		if (widget != NULL) {
 			widget->keyPressEvent(&keyEvent);
 		}
 	}
-
-	// TODO: Add options to turn button presses into KeyEvents
 }
 
 void JoystickRender::RenderJoystick::buttonReleased(int b) {
diff --git a/crosbot_ui/src/renders/robot/robotwidget.cpp b/crosbot_ui/src/renders/robot/robotwidget.cpp
index 463ddbe..7bc7492 100644
--- a/crosbot_ui/src/renders/robot/robotwidget.cpp
+++ b/crosbot_ui/src/renders/robot/robotwidget.cpp
@@ -93,9 +93,10 @@ public:
 	}
 
 	void setPos(const std::string& joint, double pos) {
-		Joint *j = findJoint(joint);
 		if (jointPub.getNumSubscribers() == 0)
 			return;
+
+		Joint *j = findJoint(joint);
 		sensor_msgs::JointState state;
 		state.header.stamp = ros::Time::now();
 		state.name.push_back(joint);
@@ -107,6 +108,24 @@ public:
 
 		jointPub.publish(state);
 	}
+
+	void zero(const std::vector< std::string >& joints) {
+		if (joints.size() == 0 || jointPub.getNumSubscribers() == 0)
+			return;
+
+		sensor_msgs::JointState state;
+		state.header.stamp = ros::Time::now();
+		for (size_t i = 0; i < joints.size(); ++i) {
+			Joint *j = findJoint(joints[i]);
+			if (j == NULL)
+				continue;
+			state.name.push_back(j->name);
+			state.position.push_back(0);
+			j->desiredPos = 0;
+		}
+
+		jointPub.publish(state);
+	}
 };
 JointController joints;
 
@@ -491,12 +510,58 @@ public:
 	bool keyReleaseEvent(QKeyEvent *e) { return false; }
 };
 
+class JointZeroKey : public RobotWidget::KeyListener {
+public:
+	std::vector< std::string > jointsToZero;
+	int key;
+
+	JointZeroKey(ConfigElementPtr cfg) : key(-1) {
+		std::string joint = cfg->getParam("joint");
+		joint = cfg->getParam("name", joint);
+
+		if (joint != "") {
+			size_t fnd = joint.find_first_of(' ', 0), last = 0;
+			if (fnd == joint.npos) {
+				jointsToZero.push_back(joint);
+			} else {
+				while (fnd != joint.npos) {
+					jointsToZero.push_back(joint.substr(last, fnd - last));
+					last = fnd + 1;
+					fnd = joint.find_first_of(' ', last);
+				}
+				std::string lastJoint = joint.substr(last);
+				if (lastJoint != "")
+					jointsToZero.push_back(lastJoint);
+			}
+		}
+
+		string str = cfg->getParam("key", "");
+		if (str != "") {
+			key = Panel::getKeyForChar(str[0]);
+		}
+	}
+
+	bool keyPressEvent(QKeyEvent *e) {
+		if (key == e->key()) {
+			joints.zero(jointsToZero);
+			return true;
+		}
+		return false;
+	}
+
+	bool keyReleaseEvent(QKeyEvent *e) { return false; }
+};
+
 void RobotWidget::addInputListener(ConfigElementPtr cfg) {
 	if (strcasecmp(cfg->name.c_str(), ELEMENT_KEY) == 0) {
 		listeners.push_back(new TopicMessageKey(cfg));
 	} else if (strcasecmp(cfg->name.c_str(), ELEMENT_JOINT) == 0) {
 		joints.connect();
-		listeners.push_back(new JointKey(cfg));
+		if (cfg->getParamAsBool("zero", false)) {
+			listeners.push_back(new JointZeroKey(cfg));
+		} else {
+			listeners.push_back(new JointKey(cfg));
+		}
 	}
 }
 
-- 
GitLab