//! Goto specified cut out position
void TTCutOutFrame::onGotoCutOut(int pos)
{
currentPosition = mpeg2Stream->moveToIndexPos(pos);
mpegWindow->showFrameAt( currentPosition );
updateCurrentPosition();
}
void AuvModelSimple6DoF::updateAuv(float force[6])
{
updateTorques(force);
updateTransformMarixes();
updateCurrentAccelaration();
updateCurrentPosition();
}
//! Goto arbitrary frame at given position
void TTCurrentFrame::onGotoFrame(int pos, int fast)
{
int newFramePos;
newFramePos = mpeg2Stream->moveToIndexPos( pos, fast );
mpegWindow->showFrameAt( newFramePos );
updateCurrentPosition();
}
//! Navigate to marker position
void TTCurrentFrame::onGotoMarker(int markerPos)
{
int newFramePos;
newFramePos = mpeg2Stream->moveToIndexPos( markerPos, 0 );
mpegWindow->showFrameAt( newFramePos );
updateCurrentPosition();
}
//! Navigate to next B-Frame
void TTCurrentFrame::onNextBFrame()
{
int newFramePos;
newFramePos = mpeg2Stream->moveToNextFrame( );
mpegWindow->showFrameAt( newFramePos );
updateCurrentPosition();
}
void Printer::setAutolevelActive(bool on) {
#if FEATURE_AUTOLEVEL
if(on == isAutolevelActive()) return;
flag0 = (on ? flag0 | PRINTER_FLAG0_AUTOLEVEL_ACTIVE : flag0 & ~PRINTER_FLAG0_AUTOLEVEL_ACTIVE);
if(on)
Com::printInfoFLN(Com::tAutolevelEnabled);
else
Com::printInfoFLN(Com::tAutolevelDisabled);
updateCurrentPosition(false);
#endif // FEATURE_AUTOLEVEL
}
//! Goto specified cut out position
void TTCutOutFrame::onGotoCutOut( int pos, TTMpeg2VideoStream* pMpeg2Stream )
{
if ( pMpeg2Stream != 0 )
initVideoStream( pMpeg2Stream );
if ( mpeg2Stream == 0 )
return;
currentPosition = mpeg2Stream->moveToIndexPos(pos);
mpegWindow->showFrameAt( currentPosition );
updateCurrentPosition();
}
//! Goto next possible cut-out position
void TTCutOutFrame::onNextCutOutPos()
{
int cutOutIndex;
if (!TTCut::encoderMode)
cutOutIndex = mpeg2Stream->moveToNextPIFrame();
else
cutOutIndex = mpeg2Stream->moveToNextFrame();
emit newCutOutFramePos(cutOutIndex);
mpegWindow->showFrameAt(cutOutIndex);
updateCurrentPosition();
}
void Scene::resize( Clip *clip, double newLength, int track )
{
QMutexLocker ml( &mutex );
if ( clip->length() == newLength )
return;
double margin = profile.getVideoFrameDuration() / 4.0;
removeTransitions( clip, track, track, tracks[track]->indexOf( clip ), clip->position(), newLength, margin );
double old = clip->position() + clip->length();
if ( clip->getSource()->getType() == InputBase::FFMPEG && clip->getSpeed() < 0 )
clip->setStart( clip->start() + ((clip->length() - newLength) * qAbs(clip->getSpeed())) );
clip->setLength( newLength );
updateTransitions( clip, track, margin );
clip->setInput( NULL );
update = updateCurrentPosition( qMin( old, clip->position() + clip->length() ), qMax( old, clip->position() + clip->length() ) );
}
void PathHandler::scanCb(const sensor_msgs::LaserScan &scan)
{
if (mFollowState != FOLLOW_STATE_BUSY || updateCurrentPosition() == false || getPathSize() == 0)
return;
uint32_t size = (scan.angle_max - scan.angle_min) / scan.angle_increment;
for (uint32_t i = 0; i < size; i++)
{
if (scan.ranges[i] >= scan.range_max)
continue;
geometry_msgs::PointStamped tmp, p;
tmp.header.frame_id = "/base_link";
tmp.header.stamp = ros::Time(0);
tmp.point.x = scan.ranges[i] * cosf(scan.angle_min + scan.angle_increment * i);
tmp.point.y = scan.ranges[i] * sinf(scan.angle_min + scan.angle_increment * i);
try
{
mTransformListener.transformPoint("/map", tmp, p);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
return;
}
for (uint32_t j = mCurrentPathIndex; j < getPathSize() - 1; j++)
{
geometry_msgs::Point closest = closestPointOnLine(p.point, mPath[j].position, mPath[j+1].position);
if (distanceBetweenPoints(p.point, closest) <= ROBOT_RADIUS)
{
ROS_INFO("Detected obstacle on path, resending goal. Distance: %f", distanceBetweenPoints(p.point, closest));
resendGoal();
return;
}
}
}
}
void Scene::resizeStart( Clip *clip, double newPos, double newLength, int track )
{
QMutexLocker ml( &mutex );
if ( clip->position() == newPos && clip->length() == newLength )
return;
double margin = profile.getVideoFrameDuration() / 4.0;
int insert, self = 0;
Track *t = tracks[track];
insert = t->clipCount();
for ( int i = 0; i < t->clipCount(); ++i ) {
Clip *c = t->clipAt( i );
if ( c == clip ) {
++self;
continue;
}
if ( newPos < c->position() ) {
insert = i;
break;
}
}
insert -= self;
double old = clip->position();
removeTransitions( clip, track, track, insert, newPos, newLength, margin );
t->removeClip( clip );
t->insertClipAt( clip, insert );
if ( clip->getSource()->getType() == InputBase::FFMPEG && clip->getSpeed() >= 0 )
clip->setStart( clip->start() + ((clip->length() - newLength) * qAbs(clip->getSpeed())) );
clip->setLength( newLength );
clip->setPosition( newPos );
updateTransitions( clip, track, margin );
clip->setInput( NULL );
update = updateCurrentPosition( qMin( old, clip->position() ), qMax( old, clip->position() ) );
}
void PaintArea::paintCurrentNode(PaintNode *currentPaintNode,
QPainter *qPainter,
std::vector<PaintNode*> *paintNodes,
std::vector<PaintNode*>::iterator
currentLocation)
{
if (currentPaintNode->getTypeOfPaintNode() == "char")
{
//Draw the text contained within each paragraph node. New lines are
//only added after each paragraph node--not any other element.
if (positionSet)
{
updateCurrentPosition();
}
char *character = currentPaintNode->getCharacter();
*currentCharacter = QString(*character);
currentFont = qPainter->font();
if (currentPaintNode->getWeight() == QFont::Bold)
{
currentFont.setBold(true);
}
else
{
currentFont.setBold(false);
}
if (currentPaintNode->getStyle() == QFont::StyleItalic)
{
currentFont.setItalic(true);
}
else
{
currentFont.setItalic(false);
}
QFontMetrics fm(currentFont);
//Wrap text by entire words, not character-by-character.
if (!nextWordChecked)
{
int currentLineWidth = totalWidth;
currentLineWidth += getNextWordWidth(paintNodes, qPainter,
currentLocation);
if (currentLineWidth + RIGHT_SIDE_PADDING + STARTING_X
>= this->width())
{
totalWidth = 0;
currentY += fm.height();
currentX = STARTING_X;
}
nextWordChecked = true;
}
QRect box(QPoint(currentX, currentY), QSize(fm.width(*character),
fm.height()));
qPainter->setFont(currentFont);
qPainter->drawText(box, Qt::AlignCenter, QString(*character));
if (isspace(*character))
{
nextWordChecked = false;
}
updateCurrentPosition();
}
else if (currentPaintNode->getTypeOfPaintNode() == "image")
{
//Draw the image.
QImage image(QString::fromStdString(currentPaintNode->getSourcePath()));
if (image.width() + totalWidth >= this->width() + RIGHT_SIDE_PADDING)
{
totalWidth = 0;
currentX = 0;
QFontMetrics fm(currentFont);
currentY += fm.height();
}
qPainter->drawImage(currentX, currentY, image);
currentX += image.width() + 10;
totalWidth += image.width() + 10;
if (currentX >= this->width() - RIGHT_SIDE_PADDING)
{
currentX = STARTING_X;
}
currentY += image.height() + 10;
nextWordChecked = false;
currentLocation++;
if ((*currentLocation)->getTypeOfPaintNode() == "node")
{
currentX = STARTING_X;
totalWidth = 0;
}
}
else if (currentPaintNode->getTypeOfPaintNode() == "hr")
{
QFontMetrics fm(currentFont);
if (currentX != STARTING_X)
{
currentX = STARTING_X;
currentY += fm.height();
}
qPainter->drawLine(QPoint(currentX, currentY),
QPoint(this->width() - RIGHT_SIDE_PADDING,
currentY));
currentY += fm.height();
totalWidth = 0;
}
else if (currentPaintNode->getTypeOfPaintNode() == "ul")
{
//This is an ugly, hard-coded solution that will be removed as part of the
//0.2.0 beta code rewrite and clean up.
insertLineBreak();
currentY -= 20;
drawDocument(qPainter, currentPaintNode->returnNode()->
getPaintNodes());
}
else if (currentPaintNode->getTypeOfPaintNode() == "li")
{
indentOn = true;
insertLineBreak();
QPen bulletPoint(Qt::black);
bulletPoint.setCapStyle(Qt::RoundCap);
bulletPoint.setWidth(5);
qPainter->setPen(bulletPoint);
qPainter->drawPoint(currentX - 7, currentY + 10);
drawDocument(qPainter, currentPaintNode->returnNode()->
getPaintNodes());
indentOn = false;
insertLineBreak();
}
else if (currentPaintNode->getTypeOfPaintNode() == "node")
{
//Call the function again on each of the PaintNode's child paint nodes.
//This ensures that all of the child nodes of the overall parent node
//are drawn.
std::vector<PaintNode*> *childPaintNodes = currentPaintNode->
returnNode()->getPaintNodes();
drawDocument(qPainter, childPaintNodes);
if (currentPaintNode->returnNode()->getTypeOfRenderNode() == "p")
{
insertLineBreak();
}
}
}
pp_int32 EnvelopeEditorControl::dispatchEvent(PPEvent* event)
{
if (eventListener == NULL)
return -1;
switch (event->getID())
{
case eRMouseDown:
{
PPPoint* p = (PPPoint*)event->getDataPtr();
if (!hScrollbar->hit(*p))
invokeContextMenu(*p);
else
{
if (controlCaughtByLMouseButton)
break;
controlCaughtByRMouseButton = true;
caughtControl = hScrollbar;
caughtControl->dispatchEvent(event);
}
break;
}
case eLMouseDown:
{
hasDragged = false;
selectionTicker = 0;
PPPoint* p = (PPPoint*)event->getDataPtr();
// Clicked on horizontal scrollbar -> route event to scrollbar and catch scrollbar control
if (hScrollbar->hit(*p))
{
if (controlCaughtByRMouseButton)
break;
controlCaughtByLMouseButton = true;
caughtControl = hScrollbar;
caughtControl->dispatchEvent(event);
}
// Clicked in client area
else
{
PPPoint cp = *p;
translateCoordinates(cp);
if (cp.x > visibleWidth)
break;
if (cp.y > visibleHeight)
break;
pp_int32 rx = (pp_int32)(cp.x + startPos);
pp_int32 ry = (pp_int32)((visibleHeight - cp.y));
pp_int32 i = selectEnvelopePoint(rx, ry);
if (i != -1)
{
envelopeEditor->startSelectionDragging(i);
}
else
{
updateCurrentPosition(cp);
}
parentScreen->paintControl(this);
}
break;
}
case eMouseLeft:
{
currentPosition.x = currentPosition.y = -1;
parentScreen->paintControl(this);
break;
}
case eMouseMoved:
{
PPPoint* p = (PPPoint*)event->getDataPtr();
PPPoint cp = *p;
translateCoordinates(cp);
if (cp.y < 0 || cp.x > visibleWidth ||
cp.y < 0 || cp.y > visibleHeight)
break;
updateCurrentPosition(cp);
parentScreen->paintControl(this);
break;
}
case eRMouseUp:
{
controlCaughtByRMouseButton = false;
if (caughtControl && !controlCaughtByLMouseButton && !controlCaughtByRMouseButton)
{
caughtControl->dispatchEvent(event);
caughtControl = NULL;
break;
}
break;
}
case eLMouseUp:
if (envelopeEditor->isSelectionDragging())
envelopeEditor->endSelectionDragging();
controlCaughtByLMouseButton = false;
if (caughtControl == NULL)
break;
if (!controlCaughtByLMouseButton && !controlCaughtByRMouseButton)
{
caughtControl->dispatchEvent(event);
caughtControl = NULL;
}
break;
case eLMouseDrag:
{
if (caughtControl && controlCaughtByLMouseButton)
{
caughtControl->dispatchEvent(event);
break;
}
hasDragged = true;
if (!envelopeEditor->isSelectionDragging())
break;
PPPoint* p = (PPPoint*)event->getDataPtr();
PPPoint cp = *p;
translateCoordinates(cp);
setEnvelopePoint(envelopeEditor->getSelectionIndex(), cp.x + startPos, visibleHeight - cp.y);
updateCurrentPosition(cp);
adjustScrollbars();
parentScreen->paintControl(this);
break;
}
case eLMouseRepeat:
{
if (caughtControl && controlCaughtByLMouseButton)
{
caughtControl->dispatchEvent(event);
break;
}
// for slowing down mouse pressed events
selectionTicker++;
// no selection has been made or mouse hasn't been dragged
if (!envelopeEditor->isSelectionDragging() || !hasDragged)
{
// mouse hasn't been dragged and selection ticker has reached threshold value
if (!hasDragged && selectionTicker > 0)
{
PPPoint* p = (PPPoint*)event->getDataPtr();
if (!hScrollbar->hit(*p))
invokeContextMenu(*p);
}
}
break;
}
case eRMouseDrag:
{
if (caughtControl && controlCaughtByRMouseButton)
caughtControl->dispatchEvent(event);
break;
}
case eRMouseRepeat:
{
if (caughtControl && controlCaughtByRMouseButton)
caughtControl->dispatchEvent(event);
break;
}
// mouse wheel
case eMouseWheelMoved:
{
TMouseWheelEventParams* params = (TMouseWheelEventParams*)event->getDataPtr();
if (params->delta > 0)
{
setScale(xScale << 1);
parentScreen->paintControl(this);
}
else if (params->delta < 0)
{
setScale(xScale >> 1);
parentScreen->paintControl(this);
}
event->cancel();
break;
}
default:
if (caughtControl == NULL)
break;
caughtControl->dispatchEvent(event);
break;
}
/**
* Updates path logic.
*/
void PathHandler::updatePath()
{
geometry_msgs::Twist command;
publishState();
// no path? nothing to handle
if (getPathSize() == 0)
return;
// update our position if possible.
if (updateCurrentPosition() == false)
{
ROS_ERROR("Failed to update robot position.");
return;
}
if (mCurrentPathIndex < getPathSize() - 1) // we are moving along a line segment in the path
{
geometry_msgs::Point robotPos;
robotPos.x = mRobotPosition.getOrigin().x();
robotPos.y = mRobotPosition.getOrigin().y();
geometry_msgs::Point closestOnPath = closestPointOnLine(robotPos, mPath[mCurrentPathIndex].position, mPath[mCurrentPathIndex + 1].position);
//ROS_INFO("robotPos[%lf, %lf], closestOnPath[%lf, %lf]", robotPos.x, robotPos.y, closestOnPath.x, closestOnPath.y);
if (distanceBetweenPoints(closestOnPath, robotPos) > mResetDistanceTolerance)
{
ROS_INFO("Drove too far away from path, re-sending goal.");
mFollowState = FOLLOW_STATE_IDLE;
resendGoal();
clearPath();
return;
}
geometry_msgs::Point pointOnPath = getPointOnPathWithDist(closestOnPath, mLookForwardDistance);
//ROS_INFO("closestOnPath[%lf, %lf], pointOnPath[%lf, %lf]", closestOnPath.x, closestOnPath.y, pointOnPath.x, pointOnPath.y);
double angle = angleTo(pointOnPath);
/*double robotYaw = tf::getYaw(mRobotPosition.getRotation()); // only used for printing
ROS_INFO("Follow state: %d Robot pos: (%lf, %lf, %lf). Target pos: (%lf, %lf, %lf). RobotYaw: %lf. FocusYaw: %lf", mFollowState, mRobotPosition.getOrigin().getX(),
mRobotPosition.getOrigin().getY(), mRobotPosition.getOrigin().getZ(), pointOnPath.x,
pointOnPath.y, pointOnPath.z, robotYaw, angle);*/
//ROS_INFO("Angle to path: %f", angle);
command.linear.x = getScaledLinearSpeed(angle);
command.angular.z = getScaledAngularSpeed(angle);
if (distanceBetweenPoints(closestOnPath, mPath[mCurrentPathIndex + 1].position) < mDistanceTolerance)
{
//ROS_INFO("Moving to next line segment on path.");
mCurrentPathIndex++;
}
publishLocalPath(command.linear.x * 3, angle);
}
else // only rotate for final yaw
{
double yaw = tf::getYaw(mPath[getPathSize() - 1].orientation);
btVector3 orientation = btVector3(cos(yaw), sin(yaw), 0.0).rotate(btVector3(0,0,1), -tf::getYaw(mRobotPosition.getRotation()));
double angle = atan2(orientation.getY(), orientation.getX());
//ROS_INFO("Angle to final orientation: %f", angle);
if (fabs(angle) > mFinalYawTolerance)
command.angular.z = getScaledAngularSpeed(angle, true);
else
{
// path is done!
mFollowState = FOLLOW_STATE_FINISHED;
clearPath();
}
publishLocalPath(1.0, angle);
}
mCommandPub.publish(command);
}
