Skip to content
Snippets Groups Projects
Commit 8f4bcbb4 authored by Rescue Group's avatar Rescue Group
Browse files

MM: Added joint zeroing

parent c2b2fc20
Branches
No related tags found
No related merge requests found
......@@ -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);
......
......@@ -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) {
......
......@@ -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,14 +510,60 @@ 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();
if (cfg->getParamAsBool("zero", false)) {
listeners.push_back(new JointZeroKey(cfg));
} else {
listeners.push_back(new JointKey(cfg));
}
}
}
} // namespace gui
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment