RobotLink::RobotLink(Robot* robot,
const urdf::LinkConstPtr& link,
const std::string& parent_joint_name,
bool visual,
bool collision,
bool use_model_path,
MetaPointCloud& link_pointclouds)
: robot_( robot )
, name_( link->name )
, parent_joint_name_( parent_joint_name )
, visual_node_( NULL )
, collision_node_( NULL )
, link_pointclouds_(link_pointclouds)
, pose_calculated_(false)
, use_model_path_(use_model_path)
{
pose_property_ = KDL::Frame();
visual_node_ = new node;
collision_node_ = new node;
// we prefer collisions over visuals
if (collision) createCollision(link);
else if (visual) createVisual(link);
// create description and fill in child_joint_names_ vector
std::stringstream desc;
if (parent_joint_name_.empty())
{
desc << "Root Link " << name_;
}
else
{
desc << "Link " << name_;
desc << " with parent joint " << parent_joint_name_;
}
if (link->child_joints.empty())
{
desc << " has no children.";
}
else
{
desc
<< " has "
<< link->child_joints.size();
if (link->child_joints.size() > 1)
{
desc << " child joints: ";
}
else
{
desc << " child joint: ";
}
std::vector<boost::shared_ptr<urdf::Joint> >::const_iterator child_it = link->child_joints.begin();
std::vector<boost::shared_ptr<urdf::Joint> >::const_iterator child_end = link->child_joints.end();
for ( ; child_it != child_end ; ++child_it )
{
urdf::Joint *child_joint = child_it->get();
if (child_joint && !child_joint->name.empty())
{
child_joint_names_.push_back(child_joint->name);
desc << child_joint->name << ((child_it+1 == child_end) ? "." : ", ");
}
}
}
if (hasGeometry())
{
if (visual_meshes_.empty())
{
desc << " This link has collision geometry but no visible geometry.";
}
else if (collision_meshes_.empty())
{
desc << " This link has visible geometry but no collision geometry.";
}
}
else
{
desc << " This link has NO geometry.";
}
LOGGING_DEBUG_C(RobotLog, RobotLink, desc << endl);
}
void X11Device::init(Device *other) {
Device::init(other);
Log(EDebug, "Initializing X11 device");
X11Session *session = static_cast<X11Session *>(getSession());
/* Find matching visuals */
m_visinfo = createVisual();
/* Fullscreen mode selection */
std::vector<XF86VidModeModeInfo *> modeList;
if (m_fullscreen) {
if (!session->m_hasVidMode)
Log(EError, "VidMode extension is required for fullscreen display");
/* Retrieve a list of all video modes */
int modeCount;
XF86VidModeModeInfo** modes;
XF86VidModeGetAllModeLines(session->m_display, session->m_screen, &modeCount, &modes);
/* Save the current mode */
m_previousMode = *modes[0];
/* Find the best matching screen resolution */
for (int i=0; i<modeCount; ++i) {
if (modes[i]->hdisplay == m_size.x &&
modes[i]->vdisplay == m_size.y)
modeList.push_back(modes[i]);
}
/* Release the memory held by the resolution list */
XFree(modes);
std::sort(modeList.begin(), modeList.end(), algo::vsync_sort());
if (modeList.size() == 0) {
Log(EWarn, "No matching fullscreen resolution found, using windowed mode!");
m_fullscreen = false;
}
}
/* Create the window's attributes */
XSetWindowAttributes x11attr;
x11attr.background_pixel = x11attr.border_pixel =
BlackPixel(session->m_display, session->m_screen);
/* Create a colormap for the window */
Colormap colormap = XCreateColormap(session->m_display, session->m_root, m_visinfo->visual, AllocNone);
x11attr.colormap = colormap;
if (m_fullscreen) {
/* Switch to the best matching resolution */
XF86VidModeSwitchToMode(session->m_display, session->m_screen, modeList[0]);
XF86VidModeSetViewPort(session->m_display, session->m_screen, 0, 0);
x11attr.override_redirect = True;
m_window = XCreateWindow(session->m_display, session->m_root,
0, 0, /* x,y position*/
m_size.x, m_size.y,
0, m_visinfo->depth, /* border width, color depth */
InputOutput, m_visinfo->visual,
CWBackPixel | CWBorderPixel | CWColormap |
CWOverrideRedirect, &x11attr);
XWarpPointer(session->m_display, None, m_window, 0, 0, 0, 0, 0, 0);
XMapRaised(session->m_display, m_window);
/* Grab the pointer & keyboard */
XGrabKeyboard(session->m_display, m_window, True,
GrabModeAsync, GrabModeAsync, CurrentTime);
XGrabPointer(session->m_display, m_window, True,
ButtonPressMask, GrabModeAsync, GrabModeAsync,
m_window, None, CurrentTime);
m_visible = true;
} else {
/* Center the window if needed */
if (m_center) {
m_position.x = (DisplayWidth(session->m_display, session->m_screen) - m_size.x) / 2;
m_position.y = (DisplayHeight(session->m_display, session->m_screen) - m_size.y) / 2;
}
/* Create the X window */
unsigned long mask = CWBackPixel | CWBorderPixel | CWColormap;
m_window = XCreateWindow(session->m_display, session->m_root,
m_position.x, m_position.y, m_size.x, m_size.y, 0, m_visinfo->depth,
InputOutput, m_visinfo->visual, mask, &x11attr);
if (!m_window)
Log(EError, "Could not create the window");
/* Make the window non-resizable */
XSizeHints *hints = XAllocSizeHints();
hints->width = m_size.x;
hints->height = m_size.y;
if (m_resizeAllowed) {
hints->min_width = hints->min_height = 10;
hints->max_width = hints->max_height = INT_MAX;
} else {
hints->min_width = hints->max_width = m_size.x;
hints->min_height = hints->max_height = m_size.y;
}
hints->x = m_position.x;
hints->y = m_position.y;
hints->flags = PMaxSize | PMinSize | USSize | USPosition;
XSetNormalHints(session->m_display, m_window, hints);
XFree(hints);
/* Set input hints */
XWMHints *wmHints = XAllocWMHints();
wmHints->input = True;
wmHints->flags = InputHint;
XSetWMHints(session->m_display, m_window, wmHints);
XFree(wmHints);
/* Make the window closeable */
m_deleteWindow = XInternAtom(session->m_display, "WM_DELETE_WINDOW", False);
XSetWMProtocols(session->m_display, m_window, &m_deleteWindow, 1);
}
/* Mark events types in which we are interested */
XSelectInput(session->m_display, m_window, FocusChangeMask | KeyPressMask | KeyReleaseMask
| ButtonPressMask | ButtonReleaseMask | PointerMotionMask | StructureNotifyMask);
/* Initialize member variables */
m_cursor = None;
m_mouse = Point2i(-1, -1);
m_modifierState = 0;
m_buttonState = 0;
m_grab = false;
m_initialized = true;
setTitle(m_title);
}
