void Q3DockArea::dockWindow(Q3DockWindow *dockWindow, DockWindowData *data) { if (!data) return; dockWindow->setParent(this); dockWindow->move(0, 0); dockWindow->installEventFilter(this); dockWindow->dockArea = this; dockWindow->updateGui(); if (dockWindows.isEmpty()) { dockWindows.append(dockWindow); } else { QList<Q3DockWindow *> lineStarts = layout->lineStarts(); int index = 0; if (lineStarts.count() > data->line) index = dockWindows.indexOf(lineStarts.at(data->line)); if (index == -1) index = 0; bool firstTime = true; int offset = data->offset; for (int i = index; i < dockWindows.size(); ++i) { Q3DockWindow *dw = dockWindows.at(i); if (!firstTime && lineStarts.contains(dw)) break; if (offset < point_pos(fix_pos(dw), orientation()) + size_extent(dw->size(), orientation()) / 2) break; index++; firstTime = false; } if (index >= 0 && index < dockWindows.count() && dockWindows.at(index)->newLine() && lineOf(index) == data->line) { dockWindows.at(index)->setNewLine(false); dockWindow->setNewLine(true); } else { dockWindow->setNewLine(false); } dockWindows.insert(index, dockWindow); } dockWindow->show(); dockWindow->setFixedExtentWidth(data->fixedExtent.width()); dockWindow->setFixedExtentHeight(data->fixedExtent.height()); updateLayout(); setSizePolicy(QSizePolicy(orientation() == Qt::Horizontal ? QSizePolicy::Expanding : QSizePolicy::Minimum, orientation() == Qt::Vertical ? QSizePolicy::Expanding : QSizePolicy::Minimum)); }
void QDockArea::dockWindow( QDockWindow *dockWindow, DockWindowData *data ) { if ( !data ) return; dockWindow->reparent( this, QPoint( 0, 0 ), FALSE ); dockWindow->installEventFilter( this ); dockWindow->dockArea = this; dockWindow->updateGui(); if ( dockWindows->isEmpty() ) { dockWindows->append( dockWindow ); } else { QPtrList<QDockWindow> lineStarts = layout->lineStarts(); int index = 0; if ( (int)lineStarts.count() > data->line ) index = dockWindows->find( lineStarts.at( data->line ) ); if ( index == -1 ) { index = 0; (void)dockWindows->at( index ); } bool firstTime = TRUE; int offset = data->offset; for ( QDockWindow *dw = dockWindows->current(); dw; dw = dockWindows->next() ) { if ( !firstTime && lineStarts.find( dw ) != -1 ) break; if ( offset < point_pos( fix_pos( dw ), orientation() ) + size_extent( dw->size(), orientation() ) / 2 ) break; index++; firstTime = FALSE; } if ( index >= 0 && index < (int)dockWindows->count() && dockWindows->at( index )->newLine() && lineOf( index ) == data->line ) { dockWindows->at( index )->setNewLine( FALSE ); dockWindow->setNewLine( TRUE ); } else { dockWindow->setNewLine( FALSE ); } dockWindows->insert( index, dockWindow ); } dockWindow->show(); dockWindow->setFixedExtentWidth( data->fixedExtent.width() ); dockWindow->setFixedExtentHeight( data->fixedExtent.height() ); updateLayout(); setSizePolicy( QSizePolicy( orientation() == Horizontal ? QSizePolicy::Expanding : QSizePolicy::Minimum, orientation() == Vertical ? QSizePolicy::Expanding : QSizePolicy::Minimum ) ); }
void PathPlanningWidget::pointAddUI() { /*! Function for adding new Way-Point from the RQT Widget. The user can set the position and orientation of the Way-Point by entering their values in the LineEdit fields. This function is connected to the AddPoint button click() signal and sends the addPoint(point_pos) to inform the RViz enviroment that a new Way-Point has been added. */ double x,y,z,rx,ry,rz; x = ui_.LineEditX->text().toDouble(); y = ui_.LineEditY->text().toDouble(); z = ui_.LineEditZ->text().toDouble(); rx = DEG2RAD(ui_.LineEditRx->text().toDouble()); ry = DEG2RAD(ui_.LineEditRy->text().toDouble()); rz = DEG2RAD(ui_.LineEditRz->text().toDouble()); // // create transform tf::Transform point_pos( tf::Transform(tf::createQuaternionFromRPY(rx,ry,rz),tf::Vector3(x,y,z))); Q_EMIT addPoint(point_pos); //ROS_INFO_STREAM("Quartenion set at point add UI: "<<q.x()<<"; "<<q.y()<<"; "<<q.z()<<"; "<<q.w()<<";"); pointRange(); }
QDockArea::DockWindowData *QDockArea::dockWindowData( QDockWindow *w ) { DockWindowData *data = new DockWindowData; data->index = findDockWindow( w ); if ( data->index == -1 ) { delete data; return 0; } QPtrList<QDockWindow> lineStarts = layout->lineStarts(); int i = -1; for ( QDockWindow *dw = dockWindows->first(); dw; dw = dockWindows->next() ) { if ( lineStarts.findRef( dw ) != -1 ) ++i; if ( dw == w ) break; } data->line = i; data->offset = point_pos( QPoint( fix_x(w), w->y() ), orientation() ); data->area = this; data->fixedExtent = w->fixedExtent(); return data; }
Q3DockArea::DockWindowData *Q3DockArea::dockWindowData(Q3DockWindow *w) { DockWindowData *data = new DockWindowData; data->index = findDockWindow(w); if (data->index == -1) { delete data; return 0; } QList<Q3DockWindow *> lineStarts = layout->lineStarts(); int i = -1; for (int j = 0; j < dockWindows.size(); ++j) { Q3DockWindow *dw = dockWindows.at(j); if (lineStarts.contains(dw)) ++i; if (dw == w) break; } data->line = i; data->offset = point_pos(QPoint(fix_x(w), w->y()), orientation()); data->area = this; data->fixedExtent = w->fixedExtent(); return data; }
bool CanvasX11::supports_gl2() { std::string gl_version_str(reinterpret_cast<const char*>(glGetString(GL_VERSION))); int gl_major(0); size_t point_pos(gl_version_str.find('.')); if (point_pos != std::string::npos) { point_pos--; size_t start_pos(gl_version_str.rfind(' ', point_pos)); if (start_pos == std::string::npos) start_pos = 0; else start_pos++; gl_major = Util::fromString<int>( gl_version_str.substr(start_pos, point_pos - start_pos + 1) ); } return gl_major >= 2; }
//extern char ROW, COLUMN; int main(int argc, char *argv[]) { frame(); printf("\033[%d;%dH", ROW+1, 1); printf("\033[0m"); printf("\033[32mscore: %d", eat_num); srandom(time(NULL)); //随机获得点的坐标,种子 point_pos(); signal(SIGALRM, sig_func); set_timer(110); //alarm(1); //定时 1 秒 system("stty -echo raw"); printf("\033[?25l"); //隐藏光标 get_dir(); // auto_route(); system("stty echo -raw"); printf("\033[0m"); //清除设置 printf("\033[?25h"); //显示光标 return 0; }
void Q3DockArea::moveDockWindow(Q3DockWindow *w, const QPoint &p, const QRect &r, bool swap) { invalidateFixedSizes(); int mse = -10; bool hasResizable = false; for (int i = 0; i < dockWindows.size(); ++i) { Q3DockWindow *dw = dockWindows.at(i); if (dw->isHidden()) continue; if (dw->isResizeEnabled()) hasResizable = true; if (orientation() != Qt::Horizontal) mse = qMax(qMax(dw->fixedExtent().width(), dw->width()), mse); else mse = qMax(qMax(dw->fixedExtent().height(), dw->height()), mse); } if (!hasResizable && w->isResizeEnabled()) { if (orientation() != Qt::Horizontal) mse = qMax(w->fixedExtent().width(), mse); else mse = qMax(w->fixedExtent().height(), mse); } Q3DockWindow *dockWindow = 0; int dockWindowIndex = findDockWindow(w); QList<Q3DockWindow *> lineStarts = layout->lineStarts(); QList<QRect> lines = layout->lineList(); bool wasAloneInLine = false; QPoint pos = mapFromGlobal(p); int line = lineOf(dockWindowIndex); QRect lr; if (line < lines.size()) lr = lines.at(line); if (dockWindowIndex != -1) { if (lineStarts.contains(w) && ((dockWindowIndex < dockWindows.count() - 1 && lineStarts.contains(dockWindows.at(dockWindowIndex + 1))) || dockWindowIndex == dockWindows.count() - 1)) wasAloneInLine = true; dockWindow = dockWindows.takeAt(dockWindowIndex); if (!wasAloneInLine) { // only do the pre-layout if the widget isn't the only one in its line if (lineStarts.contains(dockWindow) && dockWindowIndex < dockWindows.count()) dockWindows.at(dockWindowIndex)->setNewLine(true); layout->layoutItems(QRect(0, 0, width(), height()), true); } } else { dockWindow = w; bool vis = dockWindow->isVisible(); dockWindow->setParent(this); dockWindow->move(0, 0); if(vis) dockWindow->show(); if (swap) dockWindow->resize(dockWindow->height(), dockWindow->width()); w->installEventFilter(this); } lineStarts = layout->lineStarts(); lines = layout->lineList(); QRect rect = QRect(mapFromGlobal(r.topLeft()), r.size()); if (orientation() == Qt::Horizontal && QApplication::reverseLayout()) { rect = QRect(width() - rect.x() - rect.width(), rect.y(), rect.width(), rect.height()); pos.rx() = width() - pos.x(); } dockWindow->setOffset(point_pos(rect.topLeft(), orientation())); if (orientation() == Qt::Horizontal) { int offs = dockWindow->offset(); if (width() - offs < dockWindow->minimumWidth()) dockWindow->setOffset(width() - dockWindow->minimumWidth()); } else { int offs = dockWindow->offset(); if (height() - offs < dockWindow->minimumHeight()) dockWindow->setOffset(height() - dockWindow->minimumHeight()); } if (dockWindows.isEmpty()) { dockWindows.append(dockWindow); } else { int dockLine = -1; bool insertLine = false; int i = 0; QRect lineRect; // find the line which we touched with the mouse for (QList<QRect>::Iterator it = lines.begin(); it != lines.end(); ++it, ++i) { if (point_pos(pos, orientation(), true) >= point_pos((*it).topLeft(), orientation(), true) && point_pos(pos, orientation(), true) <= point_pos((*it).topLeft(), orientation(), true) + size_extent((*it).size(), orientation(), true)) { dockLine = i; lineRect = *it; break; } } if (dockLine == -1) { // outside the dock... insertLine = true; if (point_pos(pos, orientation(), true) < 0) // insert as first line dockLine = 0; else dockLine = (int)lines.count(); // insert after the last line ### size_t/int cast } else { // inside the dock (we have found a dockLine) if (point_pos(pos, orientation(), true) < point_pos(lineRect.topLeft(), orientation(), true) + 4) { // mouse was at the very beginning of the line insertLine = true; // insert a new line before that with the docking widget } else if (point_pos(pos, orientation(), true) > point_pos(lineRect.topLeft(), orientation(), true) + size_extent(lineRect.size(), orientation(), true) - 4) { // mouse was at the very and of the line insertLine = true; // insert a line after that with the docking widget dockLine++; } } if (!insertLine && wasAloneInLine && lr.contains(pos)) // if we are alone in a line and just moved in there, re-insert it insertLine = true; #if defined(QDOCKAREA_DEBUG) qDebug("insert in line %d, and insert that line: %d", dockLine, insertLine); qDebug(" (btw, we have %d lines)", lines.count()); #endif Q3DockWindow *dw = 0; if (dockLine >= (int)lines.count()) { // insert after last line dockWindows.append(dockWindow); dockWindow->setNewLine(true); #if defined(QDOCKAREA_DEBUG) qDebug("insert at the end"); #endif } else if (dockLine == 0 && insertLine) { // insert before first line dockWindows.insert(0, dockWindow); dockWindows.at(1)->setNewLine(true); #if defined(QDOCKAREA_DEBUG) qDebug("insert at the begin"); #endif } else { // insert somewhere in between // make sure each line start has a new line for (int i = 0; i < lineStarts.size(); ++i) { dw = lineStarts.at(i); dw->setNewLine(true); } // find the index of the first widget in the search line int searchLine = dockLine; #if defined(QDOCKAREA_DEBUG) qDebug("search line start of %d", searchLine); #endif Q3DockWindow *lsw = lineStarts.at(searchLine); int index = dockWindows.indexOf(lsw); if (index == -1) { // the linestart widget hasn't been found, try to find it harder if (lsw == w && dockWindowIndex <= dockWindows.count()) index = dockWindowIndex; else index = 0; } #if defined(QDOCKAREA_DEBUG) qDebug(" which starts at %d", index); #endif if (!insertLine) { // if we insert the docking widget in the existing line // find the index for the widget bool inc = true; bool firstTime = true; for (int i = index; i < dockWindows.size(); ++i) { dw = dockWindows.at(i); if (orientation() == Qt::Horizontal) dw->setFixedExtentWidth(-1); else dw->setFixedExtentHeight(-1); if (!firstTime && lineStarts.contains(dw)) // we are in the next line, so break break; if (point_pos(pos, orientation()) < point_pos(fix_pos(dw), orientation()) + size_extent(dw->size(), orientation()) / 2) { inc = false; } if (inc) index++; firstTime = false; } #if defined(QDOCKAREA_DEBUG) qDebug("insert at index: %d", index); #endif // if we insert it just before a widget which has a new line, transfer the newline to the docking widget // but not if we didn't only mave a widget in its line which was alone in the line before if (!(wasAloneInLine && lr.contains(pos)) && index >= 0 && index < dockWindows.count() && dockWindows.at(index)->newLine() && lineOf(index) == dockLine) { #if defined(QDOCKAREA_DEBUG) qDebug("get rid of the old newline and get me one"); #endif dockWindows.at(index)->setNewLine(false); dockWindow->setNewLine(true); } else if (wasAloneInLine && lr.contains(pos)) { dockWindow->setNewLine(true); } else { // if we are somewhere in a line, get rid of the newline dockWindow->setNewLine(false); } } else { // insert in a new line, so make sure the dock widget and the widget which will be after it have a newline #if defined(QDOCKAREA_DEBUG) qDebug("insert a new line"); #endif if (index < dockWindows.count()) { #if defined(QDOCKAREA_DEBUG) qDebug("give the widget at %d a newline", index); #endif Q3DockWindow* nldw = dockWindows.at(index); if (nldw) nldw->setNewLine(true); } #if defined(QDOCKAREA_DEBUG) qDebug("give me a newline"); #endif dockWindow->setNewLine(true); } // finally insert the widget dockWindows.insert(index, dockWindow); } } if (mse != -10 && w->isResizeEnabled()) { if (orientation() != Qt::Horizontal) w->setFixedExtentWidth(qMin(qMax(w->minimumWidth(), mse), w->sizeHint().width())); else w->setFixedExtentHeight(qMin(qMax(w->minimumHeight(), mse), w->sizeHint().height())); } updateLayout(); setSizePolicy(QSizePolicy(orientation() == Qt::Horizontal ? QSizePolicy::Expanding : QSizePolicy::Minimum, orientation() == Qt::Vertical ? QSizePolicy::Expanding : QSizePolicy::Minimum)); }
int main(int argc, char **argv) { ros::init(argc, argv, "lesson_move_group"); // start a background "spinner", so our node can process ROS messages // - this lets us know when the move is completed ros::AsyncSpinner spinner(1); spinner.start(); moveit::planning_interface::MoveGroup group("manipulator"); //group.setPlannerId("ompl_planning/plan_kinematic_path"); //metto il robot in home //group.setNamedTarget("home"); //group.setNamedTarget("straight_up"); group.move(); //--------------------------------- std::vector< double > Angles=group.getCurrentRPY(); Eigen::AngleAxisd rollAngle(Angles[0], Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd yawAngle(Angles[1], Eigen::Vector3d::UnitY()); Eigen::AngleAxisd pitchAngle(Angles[2], Eigen::Vector3d::UnitX()); tf::Quaternion qt; qt=tf::createQuaternionFromRPY ( Angles[0], Angles[1], Angles[2]); //abbasso il robot in posizion operativa robot_state::RobotState start_state(*group.getCurrentState()); moveit::core::RobotStatePtr kinematic_state; moveit_msgs::RobotTrajectory trajectory; std::vector<geometry_msgs::Pose> waypoints; geometry_msgs::Pose target_pose3 = group.getCurrentPose().pose; waypoints.push_back(target_pose3); //target_pose3.position.x += 0.2; target_pose3.position.z -= 0.5; waypoints.push_back(target_pose3); //waypoints.push_back(poseNew); //double fraction = group.computeCartesianPath(waypoints,CART_STEP_SIZE_,CART_JUMP_THRESH_,trajectory_,AVOID_COLLISIONS_); double fraction = group.computeCartesianPath(waypoints, 0.01, // eef_step 0.0, // jump_threshold trajectory); kinematic_state = moveit::core::RobotStatePtr(group.getCurrentState()); robot_trajectory::RobotTrajectory rt(kinematic_state->getRobotModel(), "manipulator"); rt.setRobotTrajectoryMsg(*kinematic_state, trajectory); // check to see if plan successful move_group_interface::MoveGroup::Plan plan; plan.trajectory_=trajectory; group.execute(plan); //group.move(plan) ; sleep(1.5); Angles=group.getCurrentRPY(); waypoints.clear(); target_pose3 = group.getCurrentPose().pose; waypoints.push_back(target_pose3); target_pose3.position.y += 0.5; waypoints.push_back(target_pose3); fraction = group.computeCartesianPath(waypoints, 0.01, // eef_step 0.0, // jump_threshold trajectory); kinematic_state = moveit::core::RobotStatePtr(group.getCurrentState()); //rt(kinematic_state->getRobotModel(), "manipulator"); rt.setRobotTrajectoryMsg(*kinematic_state, trajectory); plan.trajectory_=trajectory; //group.execute(plan); sleep(2.0); Angles=group.getCurrentRPY(); qt=tf::createQuaternionFromRPY ( Angles[0], Angles[1], Angles[2]); waypoints.clear(); target_pose3 = group.getCurrentPose().pose; waypoints.push_back(target_pose3); tf::Transform point_pos( tf::Transform(tf::createQuaternionFromRPY(Angles[0],Angles[1],Angles[2]-0.5),tf::Vector3(target_pose3.position.x,target_pose3.position.y,target_pose3.position.z))); tf::poseTFToMsg (point_pos,target_pose3); waypoints.push_back(target_pose3); fraction = group.computeCartesianPath(waypoints, 0.01, // eef_step 0.0, // jump_threshold trajectory); kinematic_state = moveit::core::RobotStatePtr(group.getCurrentState()); //rt(kinematic_state->getRobotModel(), "manipulator"); rt.setRobotTrajectoryMsg(*kinematic_state, trajectory); plan.trajectory_=trajectory; group.execute(plan); sleep(2.0); //ruoto il robot rispetto alla z della base group.setNamedTarget("home"); group.move(); }