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();
}
