FootstepDisplay::FootstepDisplay()
{
alpha_property_ = new rviz::FloatProperty( "Alpha", 0.5,
"0 is fully transparent, 1.0 is fully opaque.",
this, SLOT( updateAlpha() ));
show_name_property_ = new rviz::BoolProperty(
"Show Name", true,
"Show name of each footstep",
this, SLOT(updateShowName()));
use_group_coloring_property_ = new rviz::BoolProperty(
"Use Group Coloring", false,
"Use footstep_group field to colorize footsteps",
this, SLOT(updateUseGroupColoring()));
width_property_ = new rviz::FloatProperty(
"Width", 0.15,
"width of the footstep, it's not used if the dimensions is specified in Footstep message.",
this, SLOT( updateWidth() ));
height_property_ = new rviz::FloatProperty(
"height", 0.01,
"height of the footstep, it's not used if the dimensions is specified in Footstep message.",
this, SLOT( updateHeight() ));
depth_property_ = new rviz::FloatProperty(
"depth", 0.3,
"depth of the footstep, it's not used if the dimensions is specified in Footstep message.",
this, SLOT( updateDepth() ));
}
void DepthBuilderBuffered::distributeVertexDepth(Vertex localVertex)
{
const size_t startIndex = csrGraph->getStartIndex(localVertex);
const size_t endIndex = csrGraph->getEndIndex(localVertex);
for (size_t childIndex = startIndex; childIndex < endIndex; ++childIndex) {
const Vertex child = csrGraph->edges->at(childIndex)->to;
const Vertex childLocal = csrGraph->vertexToLocal(child);
const Vertex childRank = csrGraph->vertexRank(child);
const Vertex currGlobal = csrGraph->vertexToGlobal(localVertex);
if (childRank == rank) {
updateDepth(currGlobal, childLocal, depth[localVertex] + 1);
continue;
} else {
while (isSendRequestActive[childRank]) {
probeSynchData();
}
size_t &currCount = countToSend[childRank];
Vertex *&currBuffer = sendBuffer[childRank];
currBuffer[currCount] = currGlobal;
currBuffer[currCount + 1] = childLocal;
currBuffer[currCount + 2] = depth[localVertex] + 1;
currCount += elementSize;
if (currCount == sendPackageSize) {
sendData(childRank);
}
}
}
}
void DepthBuilderBuffered::processRecvData(size_t countToRead)
{
for (size_t index = 0; index < countToRead; index += elementSize) {
const Vertex parentGlobal = recvBuffer[index];
const Vertex localChild = recvBuffer[index + 1];
const Vertex newDepth = recvBuffer[index + 2];
updateDepth(parentGlobal, localChild, newDepth);
}
}
// Update Data
void Kinect::update()
{
// Update Color
updateColor();
// Update Depth
updateDepth();
// Update Fusion
updateFusion();
}
void FootstepDisplay::onInitialize()
{
MFDClass::onInitialize();
scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
line_ = new rviz::BillboardLine(context_->getSceneManager(), scene_node_);
updateShowName();
updateWidth();
updateHeight();
updateDepth();
updateAlpha();
updateUseGroupColoring();
}
// Update Data
void Kinect::update()
{
// Retrieve Multi Source Frame
ComPtr<IMultiSourceFrame> multiSourceFrame;
const HRESULT ret = multiSourceFrameReader->AcquireLatestFrame( &multiSourceFrame );
if( FAILED( ret ) ){
return;
}
// Update Color
updateColor( multiSourceFrame );
// Update Depth
updateDepth( multiSourceFrame );
}
void DiagramItem::updateGeometry()
{
prepareGeometryChange();
// calc width and height
double width = 0.0;
double height = 0.0;
QSizeF geometry = calcMinimumGeometry();
width = geometry.width();
height = geometry.height();
if (object()->isAutoSized()) {
if (!m_customIcon) {
if (width < MINIMUM_AUTO_WIDTH)
width = MINIMUM_AUTO_WIDTH;
if (height < MINIMUM_AUTO_HEIGHT)
height = MINIMUM_AUTO_HEIGHT;
}
} else {
QRectF rect = object()->rect();
if (rect.width() > width)
width = rect.width();
if (rect.height() > height)
height = rect.height();
}
// update sizes and positions
double left = -width / 2.0;
double top = -height / 2.0;
double y = top;
setPos(object()->pos());
QRectF rect(left, top, width, height);
// the object is updated without calling DiagramController intentionally.
// attribute rect is not a real attribute stored on DObject but
// a backup for the graphics item used for manual resized and persistency.
object()->setRect(rect);
if (m_customIcon) {
m_customIcon->setPos(left, top);
m_customIcon->setActualSize(QSizeF(width, height));
y += height;
}
if (m_body) {
QPolygonF bodyPolygon;
bodyPolygon
<< rect.topLeft()
<< rect.topRight() + QPointF(-FOLD_WIDTH, 0.0)
<< rect.topRight() + QPointF(0.0, FOLD_HEIGHT)
<< rect.bottomRight()
<< rect.bottomLeft();
m_body->setPolygon(bodyPolygon);
}
if (m_fold) {
QPolygonF foldPolygon;
foldPolygon
<< rect.topRight() + QPointF(-FOLD_WIDTH, 0.0)
<< rect.topRight() + QPointF(0.0, FOLD_HEIGHT)
<< rect.topRight() + QPointF(-FOLD_WIDTH, FOLD_HEIGHT);
m_fold->setPolygon(foldPolygon);
}
y += BODY_VERT_BORDER;
if (!m_customIcon) {
if (CustomIconItem *stereotypeIconItem = this->stereotypeIconItem()) {
stereotypeIconItem->setPos(left + BODY_HORIZ_BORDER, y);
y += std::max(FOLD_HEIGHT, stereotypeIconItem->boundingRect().height());
} else {
y += FOLD_HEIGHT;
}
}
if (StereotypesItem *stereotypesItem = this->stereotypesItem()) {
stereotypesItem->setPos(-stereotypesItem->boundingRect().width() / 2.0, y);
y += stereotypesItem->boundingRect().height();
}
if (m_diagramName) {
m_diagramName->setPos(-m_diagramName->boundingRect().width() / 2.0, y);
y += m_diagramName->boundingRect().height();
}
updateSelectionMarkerGeometry(rect);
updateAlignmentButtonsGeometry(rect);
updateDepth();
}
void WidgetTypes::loadWidgets(MyGUI::xml::ElementPtr _node, const std::string& _file, MyGUI::Version _version)
{
MyGUI::xml::ElementEnumerator widgets = _node->getElementEnumerator();
while (widgets.next("Widget"))
{
WidgetStyle* widget_type = getWidgetType(widgets->findAttribute("name"));
widget_type->internalType = widgets->findAttribute("internal") == "true";
// берем детей и крутимся
MyGUI::xml::ElementEnumerator field = widgets->getElementEnumerator();
while (field.next())
{
std::string key, value, group;
if (field->getName() == "Property")
{
if (!field->findAttribute("key", key))
continue;
if (!field->findAttribute("value", value))
continue;
field->findAttribute("group", group);
if (key == "Skin")
{
std::string button_name = field->findAttribute("name");
if (button_name.empty())
button_name = value;
if (group.empty())
group = DEFAULT_GOROUP_NAME;
mSkinGroups[group].push_back(SkinInfo(value, widget_type->name, button_name));
widget_type->skin.push_back(value);
}
else if (key == "DefaultSkin")
widget_type->default_skin = value;
else if (key == "Parent")
widget_type->parent = MyGUI::utility::parseBool(value);
else if (key == "Child")
widget_type->child = MyGUI::utility::parseBool(value);
else if (key == "Resizeable")
widget_type->resizeable = MyGUI::utility::parseBool(value);
else if (key == "ItemManager")
widget_type->many_items = MyGUI::utility::parseBool(value);
else if (key == "Base")
widget_type->base = value;
}
else if (field->getName() == "Parameter")
{
if (!field->findAttribute("key", key))
continue;
if (!field->findAttribute("value", value))
continue;
widget_type->parameter.push_back(MyGUI::PairString(key, value));
}
else if (field->getName() == "TemplateData")
{
if (!field->findAttribute("key", key))
continue;
if (!field->findAttribute("value", value))
continue;
widget_type->templateData.push_back(MyGUI::PairString(key, value));
}
else if (field->getName() == "ParameterData")
{
if (!field->findAttribute("key", key))
continue;
if (!field->findAttribute("value", value))
continue;
widget_type->parameterData.push_back(MyGUI::PairString(key, value));
}
}
if (widget_type->base.empty() && widget_type->name != "Widget")
widget_type->base = "Widget";
}
updateDepth();
}
void WidgetTypes::updateDepth()
{
for (VectorWidgetType::iterator item = mWidgetTypes.begin(); item != mWidgetTypes.end(); ++ item)
(*item)->depth = updateDepth(*item);
}
bool SingleBeamScanner::gotoElement(int x, int y, bool ignoreDepth)
{
std::cout << "gotoElement (" << x << "," << y << ")" << std::endl;
Element* target = polygon->matrix[x][y];
double targetX = target->getX();
double targetY = target->getY();
//DEBUG
//if(rand() % 5 == 0 && !ignoreDepth && target->getStatus() != 1)
//return false;
/*
int cx = polygon->nx/3;
int cy = polygon->ny/3;
if(sqrt((x-cx)*(x-cx)+(y-cy)*(y-cy))<3 && !ignoreDepth)
return false;
cx = 2*polygon->nx/3;
cy = 2*polygon->ny/3;
if(sqrt((x-cx)*(x-cx)+(y-cy)*(y-cy))<3 && !ignoreDepth)
return false;
if(((polygon->nx-x)*(polygon->nx-x)+(polygon->ny-y)*(polygon->ny-y)) < 100 && !ignoreDepth)
return false;
if(((x)*(x)+(polygon->ny-y)*(polygon->ny-y)) < 256 && !ignoreDepth)
return false;
if( x == 20 && y<10 && !ignoreDepth)
return false;
if( x == 15 && y<10 && !ignoreDepth)
return false;
if( (polygon->nx-x) < 8 && y==13 && !ignoreDepth)
return false;
if( (polygon->nx-x) < 5 && y==10 && !ignoreDepth)
return false;
if( (polygon->nx -x) < 5 && y==7 && !ignoreDepth)
return false;
*/
data->setBoatWaypoint_local(0,0,targetX,targetY,1,true);
double currentX;
double currentY;
int askforpath = 1;
while(!stop)
{
usleep(delay);
if((askforpath=(++askforpath)%10) == 0)
data->askForPath();
currentX = data->getX();
currentY = data->getY();
//check if path is correct
if(!data->hasCorrectPath(0,0,data->yTOlat(targetY),data->xTOlon(targetX),targetSpeed) && askforpath != 0)
{
std::cout << "wrong path, sending path again" << std::endl;
data->setBoatWaypoint_local(0,0,targetX,targetY,targetSpeed,true);
}
double d = sqrt((targetX-currentX)*(targetX-currentX) + (targetY-currentY)*(targetY-currentY));
std::cout << "Distance left to target:" << d << "m" << std::endl;
std::cout << "Depth: " << data->getDepth() << std::endl;
if(d<tol)
{
updateDepth(currentX,currentY,data->getDepth(),true);
return true;
}
else
{
updateDepth(currentX,currentY,data->getDepth(),false);
}
//if(0.5*(data->getDepth_Right()+data->getDepth_Left()) < depthThreshold)
if(data->getDepth() < depthThreshold)
{
std::cout << "near land" << std::endl;
std::cout << "Down :" << data->getDepth() << std::endl;
std::cout << "Right :" << data->getDepth_Right() << std::endl;
std::cout << "Left :" << data->getDepth_Left() << std::endl;
if(!ignoreDepth)
return false;
}
}
//target->updateDepth(5);
polygon->updateView(data->getX(),data->getY(),data->getHeading());
return true;
}
void ItemItem::updateGeometry()
{
prepareGeometryChange();
// calc width and height
double width = 0.0;
double height = 0.0;
QSizeF geometry = calcMinimumGeometry();
width = geometry.width();
height = geometry.height();
if (object()->isAutoSized()) {
// nothing
} else {
QRectF rect = object()->rect();
if (rect.width() > width)
width = rect.width();
if (rect.height() > height)
height = rect.height();
}
// update sizes and positions
double left = -width / 2.0;
double right = width / 2.0;
double top = -height / 2.0;
double y = top;
setPos(object()->pos());
QRectF rect(left, top, width, height);
// the object is updated without calling DiagramController intentionally.
// attribute rect is not a real attribute stored on DObject but
// a backup for the graphics item used for manual resized and persistency.
object()->setRect(rect);
if (m_customIcon) {
m_customIcon->setPos(left, top);
m_customIcon->setActualSize(QSizeF(width, height));
}
if (m_shape)
m_shape->setRect(rect);
if (m_customIcon) {
switch (shapeIcon().textAlignment()) {
case qmt::StereotypeIcon::TextalignBelow:
y += height + BODY_VERT_BORDER;
break;
case qmt::StereotypeIcon::TextalignCenter:
{
double h = 0.0;
if (CustomIconItem *stereotypeIconItem = this->stereotypeIconItem())
h += stereotypeIconItem->boundingRect().height();
if (StereotypesItem *stereotypesItem = this->stereotypesItem())
h += stereotypesItem->boundingRect().height();
if (nameItem())
h += nameItem()->boundingRect().height();
if (m_contextLabel)
h += m_contextLabel->height();
y = top + (height - h) / 2.0;
break;
}
case qmt::StereotypeIcon::TextalignNone:
// nothing to do
break;
case qmt::StereotypeIcon::TextalignTop:
y += BODY_VERT_BORDER;
break;
}
} else {
y += BODY_VERT_BORDER;
}
if (CustomIconItem *stereotypeIconItem = this->stereotypeIconItem()) {
stereotypeIconItem->setPos(right - stereotypeIconItem->boundingRect().width() - BODY_HORIZ_BORDER, y);
y += stereotypeIconItem->boundingRect().height();
}
if (StereotypesItem *stereotypesItem = this->stereotypesItem()) {
stereotypesItem->setPos(-stereotypesItem->boundingRect().width() / 2.0, y);
y += stereotypesItem->boundingRect().height();
}
if (nameItem()) {
nameItem()->setPos(-nameItem()->boundingRect().width() / 2.0, y);
y += nameItem()->boundingRect().height();
}
if (m_contextLabel) {
if (m_customIcon) {
m_contextLabel->resetMaxWidth();
} else {
double maxContextWidth = width - 2 * BODY_HORIZ_BORDER;
m_contextLabel->setMaxWidth(maxContextWidth);
}
m_contextLabel->setPos(-m_contextLabel->boundingRect().width() / 2.0, y);
}
updateSelectionMarkerGeometry(rect);
updateRelationStarterGeometry(rect);
updateAlignmentButtonsGeometry(rect);
updateDepth();
}
