diff --git a/crosbot/package.xml b/crosbot/package.xml index 4ac5ef76facb5037f42bfc5b8c1eecb2a8bf9152..7aa419492ecbadfdbfdcc2091a09728f85629535 100644 --- a/crosbot/package.xml +++ b/crosbot/package.xml @@ -46,16 +46,16 @@ tf geometry_msgs sensor_msgs - jpeg - xml2 + roscpp bullet tf geometry_msgs sensor_msgs - jpeg - xml2 + @@ -66,4 +66,4 @@ - \ No newline at end of file + diff --git a/crosbot_ui/CMakeLists.txt b/crosbot_ui/CMakeLists.txt index 1c970b1ee746193b4fed1053dcff28f8cdf7c932..9ac440c97d026f84d82a65e4343115d3100ddefb 100644 --- a/crosbot_ui/CMakeLists.txt +++ b/crosbot_ui/CMakeLists.txt @@ -90,6 +90,7 @@ add_library(crosbot_ui src/renders/robot/image.cpp src/renders/robot/joystick.cpp src/renders/robot/preset.cpp + src/renders/robot/jointcontrol.cpp ${QT_MOC_HPP} ${QT_RESOURCES_CPP} ) diff --git a/crosbot_ui/include/crosbot_ui/panels/robot.hpp b/crosbot_ui/include/crosbot_ui/panels/robot.hpp index e3be2cf65c0ea10558003ff570d9ec01271e4652..1acf71f9af3fa5a78fc340ae5935a2893354c598 100644 --- a/crosbot_ui/include/crosbot_ui/panels/robot.hpp +++ b/crosbot_ui/include/crosbot_ui/panels/robot.hpp @@ -90,6 +90,8 @@ protected: ros::Publisher* drivePub; float pan, tilt, zoom, desiredPan, desiredTilt, desiredZoom; +public: + RobotWidget& getRobotWidget() { return widget; } }; } // namespace gui diff --git a/crosbot_ui/include/crosbot_ui/renders/robot/preset.hpp b/crosbot_ui/include/crosbot_ui/renders/robot/preset.hpp index 758a8d8eacf299a21232d4e61e248058e2fb7671..74002a06c987a2e1e2db9b3ea7ee28d513197947 100644 --- a/crosbot_ui/include/crosbot_ui/renders/robot/preset.hpp +++ b/crosbot_ui/include/crosbot_ui/renders/robot/preset.hpp @@ -15,6 +15,9 @@ namespace crosbot { namespace gui { #define RENDER_PRESET "preset" +#define RENDER_EMU "emu" + +#define VALID_PRESET(P) ((P) != INFINITY && (P) != -INFINITY && (P) != NAN) class PresetRender : public RobotRender { public: @@ -32,6 +35,20 @@ protected: std::vector< std::vector > presets; }; +class EmuRender : public PresetRender { +protected: + GLuint displayLists; + bool displayListsSet; +public: + EmuRender(RobotPanel& panel, ConfigElementPtr config); + void start() {} + void stop() {} + + void renderRobot(std::string label, std::vector configuration, bool renderGoals = false); + std::vector getConfiguration(); + void setConfiguration(std::vector); +}; + } // namespace gui } // namespace crosbot diff --git a/crosbot_ui/src/panels/robot.cpp b/crosbot_ui/src/panels/robot.cpp index 92fa69d783531021655b402dbedf4613cae16c55..23584dd2e3f366a44120fffee072f22dacbec832 100644 --- a/crosbot_ui/src/panels/robot.cpp +++ b/crosbot_ui/src/panels/robot.cpp @@ -7,11 +7,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -105,6 +106,8 @@ RobotRender *RobotPanel::getRender(ConfigElementPtr config, RobotPanel& panel) { } else if (strcasecmp(config->name.c_str(), "junk") == 0) { rval = new JointRender(panel, config); #endif + } else if (strcasecmp(config->name.c_str(), "emu") == 0) { + rval = new EmuRender(panel, config); } } diff --git a/crosbot_ui/src/renders/robot/preset.cpp b/crosbot_ui/src/renders/robot/preset.cpp index 8723ae208ebccdbbaf2741eca413f2071ccbe01a..1408c6de4dfbbc4b323258f5c60d8e044d3eecfe 100644 --- a/crosbot_ui/src/renders/robot/preset.cpp +++ b/crosbot_ui/src/renders/robot/preset.cpp @@ -6,18 +6,64 @@ */ #include +#include -#include +#include namespace crosbot { namespace gui { +std::vector getParamAsList(std::string param) { + std::vector rval; + + std::string elem; + char c[2], cOld = '\0'; + c[1] = '\0'; + for (unsigned int i = 0; i < param.size(); i++) { + c[0] = param.at(i); + if (isspace(c[0])) { + if (cOld != '\0') { + rval.push_back(elem); + elem = ""; + cOld = '\0'; + } + } else { + elem.append(c); + cOld = c[0]; + } + } + if (cOld != '\0') { + rval.push_back(elem); + elem = ""; + cOld = '\0'; + } + + return rval; +} + PresetRender::PresetRender(RobotPanel& panel, ConfigElementPtr config) : - RobotRender(panel, config), - renderPresets(true), layout(1) + RobotRender(panel, config, QRectF(0, 0, 1, 0.2)), + renderPresets(true), layout(2) { presets.resize(10); + int setPresets = 0; + for (size_t i = 0; i < config->getChildCount() && setPresets < 10; ++i) { + ConfigElementPtr child = config->getChild(i); + + if (strcasecmp(child->name.c_str(), "preset") == 0) { + std::string set = child->getParam("value"); + std::vector< std::string > angs = getParamAsList(set); + + if (angs.size() > 0) { + std::vector preset; + for (size_t j = 0; j < angs.size(); ++j) { + preset.push_back(atof(angs[j].c_str())); + } + presets[setPresets++] = preset; + } + } + } } void PresetRender::render() { @@ -105,6 +151,277 @@ bool PresetRender::keyPressEvent(QKeyEvent *ke) { return false; } +EmuRender::EmuRender(RobotPanel& panel, ConfigElementPtr config) : PresetRender(panel, config) {} + +std::vector EmuRender::getConfiguration() { + std::vector rval; + rval.push_back(RAD2DEG(JointController::getPos("arm_base"))); + rval.push_back(RAD2DEG(JointController::getPos("arm_shoulder"))); + rval.push_back(RAD2DEG(JointController::getPos("arm_elbow"))); + rval.push_back(RAD2DEG(JointController::getPos("neck_tilt"))); + rval.push_back(RAD2DEG(JointController::getPos("neck_pan"))); + return rval; +} + +void EmuRender::setConfiguration(std::vector preset) { + if (preset.size() > 0 && VALID_PRESET(preset[0])) { + float armAngle = DEG2RAD(preset[0]); + JointController::setPos("arm_base", preset[0]); + } + if (preset.size() > 1 && VALID_PRESET(preset[1])) { + float armAngle = DEG2RAD(preset[1]); + JointController::setPos("arm_shoulder", preset[1]); + } + if (preset.size() > 2 && VALID_PRESET(preset[2])) { + float armAngle = DEG2RAD(preset[0]); + JointController::setPos("arm_elbow", preset[2]); + } + if (preset.size() > 3 && VALID_PRESET(preset[3])) { + float armAngle = DEG2RAD(preset[3]); + JointController::setPos("neck_tilt", preset[3]); + } + if (preset.size() > 4 && VALID_PRESET(preset[4])) { + float armAngle = DEG2RAD(preset[4]); + JointController::setPos("neck_pan", preset[4]); + } +} + +void EmuRender::renderRobot(std::string label, std::vector configuration, bool renderGoals) { + // Set the colours used in the render + float lightpos[4] = {0,0,0,1}; + float white[4] = {1.0, 1.0f, 1.0f,1.0f}; + float baseMat[4] = {0,1,1.0f, 0.6f}; + float wheelMat[4] = {0.9,0,1.0f, 0.6f}; + float armMat[4] = {0,0,1.0f, 0.6f}; + float goalMat[4] = {1.0,0,0,0.6f}; + + // The robot's measurements + float baseWidth = 0.32, baseLength = 0.36, baseHeight = 0.14; + float wheelHeight = 0.24, wheelWidth = .08, wheelClearence = 0.09, wheelSpacing = 0.02; + float cubeSide = 0.07;//, cubeFromRightSide = 0.08; -- not used + float armWidth = 0.04, armDepth = 0.02, armLength = 0.60; + float headWidth = 0.14, headLength = 0.17, headHeight = 0.09; + float headFromArm = 0.03; + + float baseA = JointController::getPos("arm_base"), + shoulderA = JointController::getPos("arm_shoulder"), + elbowA = JointController::getPos("arm_elbow"), + tiltA = JointController::getPos("neck_tilt"), + panA = JointController::getPos("neck_pan"); +// PanTiltZoom ptz = panel->getCurrentPanTilt(); + + + if (configuration.size() > 0 && VALID_PRESET(configuration[0])) { + baseA = DEG2RAD(configuration[0]); + } + if (configuration.size() > 1 && VALID_PRESET(configuration[1])) { + shoulderA = DEG2RAD(configuration[1]); + } + if (configuration.size() > 2 && VALID_PRESET(configuration[2])) { + elbowA = DEG2RAD(configuration[2]); + } + if (configuration.size() > 3 && VALID_PRESET(configuration[3])) { + tiltA = DEG2RAD(configuration[3]); + } + if (configuration.size() > 4 && VALID_PRESET(configuration[4])) { + panA = DEG2RAD(configuration[4]); + } + + // Origin is where the arm joins the base + + if(!displayListsSet){ + displayLists = glGenLists(6); + //The Base of the robot + glNewList(displayLists, GL_COMPILE); + glPushMatrix(); + glTranslatef(0,-baseHeight/2-cubeSide/2, -(baseLength-cubeSide)/2); + glScalef(baseWidth, baseHeight, baseLength); + glutSolidCube(1); + glPopMatrix(); +// glEnd(); + glEndList(); + + + // The power cubes + glNewList(displayLists+1, GL_COMPILE); + glPushMatrix(); + glScalef(cubeSide, cubeSide, cubeSide); + glutSolidCube(1); + glPopMatrix(); + glEndList(); + + + // The robots arm + glNewList(displayLists+2, GL_COMPILE); + glPushMatrix(); + glTranslatef(0, armLength/2 - cubeSide/2, (armDepth+cubeSide)/2); + glScalef(armWidth, armLength, armDepth); + glutSolidCube(1); + glPopMatrix(); + glEndList(); + + //The sensor head + glNewList(displayLists+3, GL_COMPILE); + glPushMatrix(); + glTranslatef(0, headHeight/2 + headFromArm, 0); + //Rays show fov roughly + glDisable(GL_LIGHTING); + glColor3f(1,1,0); + glBegin(GL_LINES); + glVertex3f(0,0,0); + glVertex3f(0,0,0.6); + glEnd(); + glEnable(GL_LIGHTING); + glScalef(headWidth, headHeight, headLength); + // glScalef(0.11, 0.13, 0.09); + glutSolidCube(1); + glPopMatrix(); + glEndList(); + + // The Wheels + glNewList(displayLists+4, GL_COMPILE); + glPushMatrix(); + GLUquadric *quad = gluNewQuadric(); + gluQuadricNormals(quad, GLU_SMOOTH); + gluQuadricTexture(quad, GLU_TRUE); + gluQuadricDrawStyle(quad, GLU_FILL); + + glScalef(wheelWidth, wheelHeight, wheelHeight); + glRotatef(90,0,1,0); + + gluCylinder(quad, 0.5, 0.5, 1, 12, 1); + gluDisk(quad,0,0.5,12,1); + glTranslatef(0, 0, 1); + gluDisk(quad,0,0.5,12,1); + + gluDeleteQuadric(quad); + glPopMatrix(); + glEndList(); + + displayListsSet = true; + } + + glEnable(GL_NORMALIZE); + glEnable(GL_RESCALE_NORMAL); + glEnable(GL_DEPTH_TEST); + glShadeModel(GL_SMOOTH); + + glPushMatrix(); + glScalef(0.6,0.6,0.6); + glTranslatef(1,1,0); + glScalef(-1.5,1.5,1.5); + glLightfv(GL_LIGHT0, GL_POSITION, lightpos); + glTranslatef(0.25,-0.25,1); + glRotatef(45,1,0,0); + glRotatef(-135,0,1,0); + glColor4f(0,1,0,0.5); + glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 1); + glLightfv(GL_LIGHT0, GL_DIFFUSE, white); + + glTranslatef(0,-0.2,0.4); + // Draw Robot + glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, baseMat); + glEnable(GL_LIGHT0); + glEnable(GL_LIGHTING); + glCallList(displayLists); + + // draw fixed power cube + glPushMatrix(); + glTranslatef(cubeSide/2, 0, 0); + + glCallList(displayLists + 1); + glPopMatrix(); + + // Draw wheels + glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, wheelMat); + glPushMatrix(); + float ymove = -cubeSide/2 - baseHeight + (wheelHeight /2 - wheelClearence); + float zmove1 = (-wheelHeight - wheelSpacing + baseLength - cubeSide)/2; + float zmove2 = zmove1 - (wheelHeight + wheelSpacing); + glPushMatrix(); + glTranslatef(baseWidth/2, ymove, zmove1); + glCallList(displayLists + 4); + glPopMatrix(); + glPushMatrix(); + glTranslatef(-baseWidth/2-wheelWidth, ymove, zmove1); + glCallList(displayLists + 4); + glPopMatrix(); + glPushMatrix(); + glTranslatef(baseWidth/2, ymove, zmove2); + glCallList(displayLists + 4); + glPopMatrix(); + glPushMatrix(); + glTranslatef(-baseWidth/2-wheelWidth, ymove, zmove2); + glCallList(displayLists + 4); + glPopMatrix(); + glPopMatrix(); + + // draw power cube + glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, armMat); + glPushMatrix(); + glRotatef(RAD2DEG(shoulderA), 1, 0, 0); + glTranslatef(-cubeSide/2, 0, 0); + + glCallList(displayLists + 1); + glPopMatrix(); + + // draw arm + glPushMatrix(); + glRotatef(RAD2DEG(shoulderA),1,0,0); + glCallList(displayLists + 2); + glPopMatrix(); + + // draw head + glPushMatrix(); + float distFromOrigin = armLength-cubeSide/2; + glTranslatef(0, cos(shoulderA)*distFromOrigin, sin(shoulderA)*distFromOrigin); + glRotatef(-RAD2DEG(tiltA),1,0,0); + glRotatef(-RAD2DEG(panA),0,1,0); + glCallList(displayLists + 3); + glPopMatrix(); + + // draw head + // If wer're drawing goal positions +// if(renderGoals){ +// float goalArm = panel->getDesiredPos(armName); +// PanTiltZoom goalPTZ = panel->getDesiredPanTilt(); +// +// glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, goalMat); +// +// // draw power cube +// glPushMatrix(); +// glRotatef(RAD2DEG(-goalArm), 1, 0, 0); +// glTranslatef(-cubeSide/2, 0, 0); +// +// glCallList(displayLists + 1); +// glPopMatrix(); +// +// // draw arm +// glPushMatrix(); +// glRotatef(RAD2DEG(-goalArm),1,0,0); +// glCallList(displayLists + 2); +// glPopMatrix(); +// +// // draw head +// glPushMatrix(); +// float distFromOrigin = armLength-cubeSide/2; +// glTranslatef(0, cos(goalArm)*distFromOrigin, sin(-goalArm)*distFromOrigin); +// glRotatef(-RAD2DEG(goalPTZ.tilt),1,0,0); +// glRotatef(-RAD2DEG(goalPTZ.pan),0,1,0); +// glCallList(displayLists + 3); +// glPopMatrix(); +// } + glPopMatrix(); + + glDisable(GL_LIGHTING); + glDisable(GL_LIGHT0); + glColor3f(1,1,1); + panel.getRobotWidget().renderText(0.8f,0.0f,0.0f,QString(label.c_str()),QFont("Helvetica", 8)); + glDisable(GL_RESCALE_NORMAL); + glDisable(GL_NORMALIZE); + glDisable(GL_DEPTH_TEST); +} + } // namespace gui } // namespace crosbot diff --git a/crosbot_ui/src/renders/robot/robotwidget.cpp b/crosbot_ui/src/renders/robot/robotwidget.cpp index 0edc112a43f8db5b992739e6fefc1fb43a6df252..330db2413d19b5ab353d9746c180ef337c70f407 100644 --- a/crosbot_ui/src/renders/robot/robotwidget.cpp +++ b/crosbot_ui/src/renders/robot/robotwidget.cpp @@ -8,11 +8,11 @@ //#include #include #include +#include -#include -#include +#include +#include #include -#include #include @@ -21,114 +21,6 @@ namespace crosbot { namespace gui { using namespace std; -class JointController { -protected: - ros::Subscriber jointSub; - ros::Publisher jointPub; - bool connected; - - class Joint { - public: - std::string name; - double pos, desiredPos; - Joint(const std::string& name) : name(name), pos(NAN), desiredPos(NAN) {} - }; - - - Joint& getJoint(); - ReadWriteLock jLock; - std::vector< Joint > joints; -public: - JointController() : connected(false) {} - - void shutdown() { - jointPub.shutdown(); - jointSub.shutdown(); - connected = false; - } - - Joint *findJoint(const std::string& joint) { - Lock lock(jLock); - for (size_t i = 0; i < joints.size(); ++i) { - if (joints[i].name == joint) { - return &joints[i]; - } - } - return NULL; - } - - void callbackJoints(const sensor_msgs::JointStateConstPtr& state) { - for (size_t i = 0; i < state->name.size(); ++i) { - Joint *joint = findJoint(state->name[i]); - - {{ - Lock lock(jLock, true); - if (joint == NULL) { - joints.push_back(Joint(state->name[i])); - joint = &joints[joints.size() - 1]; - } - joint->pos = state->position[i]; - }} - } - } - - void connect() { - if (connected) - return; - - ros::NodeHandle nh; - jointSub = nh.subscribe("/joint_states", 2, &JointController::callbackJoints, this); - jointPub = nh.advertise< sensor_msgs::JointState >("/joint_control", 1); - } - - double getPos(const std::string& joint) { - Lock lock(jLock); - for (size_t i = 0; i < joints.size(); ++i) { - Joint& j = joints[i]; - if (j.name == joint) - return j.pos; - } - - return NAN; - } - - void setPos(const std::string& joint, double pos) { - if (jointPub.getNumSubscribers() == 0) - return; - - Joint *j = findJoint(joint); - sensor_msgs::JointState state; - state.header.stamp = ros::Time::now(); - state.name.push_back(joint); - state.position.push_back(pos); - - if (j != NULL) { - j->desiredPos = pos; - } - - 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; - RobotWidget::RobotWidget(RobotPanel& panel) : panel(panel) { aspectRatio = 1.0; updateInterval = 50; @@ -498,10 +390,10 @@ public: bool keyPressEvent(QKeyEvent *e) { if (up == e->key()) { - joints.setPos(joint, joints.getPos(joint) + step); + JointController::setPos(joint, JointController::getPos(joint) + step); return true; } else if (down == e->key()) { - joints.setPos(joint, joints.getPos(joint) - step); + JointController::setPos(joint, JointController::getPos(joint) - step); return true; } return false; @@ -543,7 +435,7 @@ public: bool keyPressEvent(QKeyEvent *e) { if (key == e->key()) { - joints.zero(jointsToZero); + JointController::zero(jointsToZero); return true; } return false; @@ -556,7 +448,7 @@ 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(); + JointController::connect(); if (cfg->getParamAsBool("zero", false)) { listeners.push_back(new JointZeroKey(cfg)); } else { @@ -573,7 +465,7 @@ JointRender::JointRender(RobotPanel& panel, ConfigElementPtr config) : {} void JointRender::start() { - joints.connect(); + JointController::connect(); } void JointRender::stop() {} @@ -588,8 +480,8 @@ void JointRender::render() { char ascii[4096]; sprintf(ascii, "b: %3.0lf s: %3.0lf e: %3.0lf t: %3.0lf p: %3.0lf", - RAD2DEG(joints.getPos("arm_base")), RAD2DEG(joints.getPos("arm_shoulder")), RAD2DEG(joints.getPos("arm_elbow")), - RAD2DEG(joints.getPos("neck_tilt")), RAD2DEG(joints.getPos("neck_pan"))); + RAD2DEG(JointController::getPos("arm_base")), RAD2DEG(JointController::getPos("arm_shoulder")), RAD2DEG(JointController::getPos("arm_elbow")), + RAD2DEG(JointController::getPos("neck_tilt")), RAD2DEG(JointController::getPos("neck_pan"))); QString label(""); label.append(ascii);