void MotionPlanningFrame::copyTrajectoryToDisplay(const moveit_msgs::RobotState& start_state,
const apc_msgs::PrimitivePlan& plan)
{
// Get a robot model.
const robot_model::RobotModelConstPtr& robot_model = planning_display_->getRobotModel();
// Construct a new robot trajectory.
robot_trajectory::RobotTrajectoryPtr display_trajectory(new robot_trajectory::RobotTrajectory(robot_model, ""));
// Accumulate joint trajectory over entire plan.
trajectory_msgs::JointTrajectory trajectory = trajectory_msgs::JointTrajectory();
for (int i = 0; i < plan.actions.size(); i++)
appendToTrajectory(trajectory, plan.actions[i].joint_trajectory);
// Copy current plan over to robot trajectory.
display_trajectory->setRobotTrajectoryMsg(planning_display_->getPlanningSceneRO()->getCurrentState(),
// FIXME start_state,
trajectory);
// Swap the plan trajectory into our planning display.
planning_display_->setTrajectoryToDisplay(display_trajectory);
// Display trail. FIXME This doesn't accomplish anything actually.
previewButtonClicked();
}
void MotionPlanningFrame::connectTeleopSlots()
{
connect( ui_->plan_button, SIGNAL( clicked() ), this, SLOT( planButtonClicked() ));
connect( ui_->execute_button, SIGNAL( clicked() ), this, SLOT( executeButtonClicked() ));
connect( ui_->preview_button, SIGNAL( clicked() ), this, SLOT( previewButtonClicked() ));
connect( ui_->stop_button, SIGNAL( clicked() ), this, SLOT( stopButtonClicked() ));
}
QWidget* Sis3350UI::createButtons()
{
QWidget* box = new QWidget();
QGridLayout* l = new QGridLayout();
QLabel* modeLabel = new QLabel(tr("Mode:"));
modeBox = new QComboBox();
modeBox->addItem(tr("Ringbuffer Async"));
modeBox->addItem(tr("Ringbuffer Sync"));
modeBox->addItem(tr("Direct Mem Gate Async"));
modeBox->addItem(tr("Direct Mem Gate Sync"));
modeBox->addItem(tr("Direct Memory Stop"));
modeBox->addItem(tr("Direct Memory Start"));
connect(modeBox,SIGNAL(currentIndexChanged(int)),this,SLOT(modeChanged(int)));
QLabel* addressLabel = new QLabel(tr("Base Address:"));
baseAddressEdit = new QLineEdit(tr("%1").arg(module->conf.base_addr,8,16,QChar('0')));
baseAddressEdit->setReadOnly(true);
autoUpdateCheckbox = new QCheckBox(tr("Auto update"));
singleShot = new QPushButton(tr("Update"));
connect(singleShot,SIGNAL(clicked()),this,SLOT(singleShotClicked()));
freeRunningButton = new QPushButton(tr("Free Running"));
freeRunningButton->setCheckable (true);
connect(freeRunningButton,SIGNAL(clicked()),this,SLOT(freeRunningButtonClicked()));
previewButton = new QPushButton(tr("Preview..."));
connect(previewButton,SIGNAL(clicked()),this,SLOT(previewButtonClicked()));
l->addWidget(modeLabel,0,0,1,1);
l->addWidget(modeBox,0,1,1,1);
l->addWidget(addressLabel,0,2,1,1);
l->addWidget(baseAddressEdit,0,3,1,1);
l->addWidget(singleShot,1,0,1,1);
l->addWidget(freeRunningButton,1,1,1,1);
l->addWidget(previewButton,1,2,1,1);
l->addWidget(autoUpdateCheckbox,1,3,1,1);
box->setLayout(l);
return box;
}
PrintDialog::PrintDialog( QWidget* parent, ScribusDoc* doc, const PrintOptions& printOptions, bool gcr, QStringList spots)
: QDialog( parent )
{
setupUi(this);
setModal(true);
cdia = 0;
m_doc = doc;
unit = doc->unitIndex();
unitRatio = unitGetRatioFromIndex(doc->unitIndex());
prefs = PrefsManager::instance()->prefsFile->getContext("print_options");
DevMode = printOptions.devMode;
PrinterOpts = "";
setWindowIcon(QIcon(loadIcon("AppIcon.png")));
pageNrButton->setIcon(QIcon(loadIcon("ellipsis.png")));
printEngines->addItem( CommonStrings::trPostScript1 );
printEngines->addItem( CommonStrings::trPostScript2 );
printEngines->addItem( CommonStrings::trPostScript3 );
markLength->setNewUnit(unit);
markLength->setMinimum(1*unitRatio);
markLength->setMaximum(3000*unitRatio);
markOffset->setNewUnit(unit);
markOffset->setMinimum(0);
markOffset->setMaximum(3000*unitRatio);
BleedBottom->setNewUnit(unit);
BleedBottom->setMinimum(0);
BleedBottom->setMaximum(3000*unitRatio);
BleedLeft->setNewUnit(unit);
BleedLeft->setMinimum(0);
BleedLeft->setMaximum(3000*unitRatio);
BleedRight->setNewUnit(unit);
BleedRight->setMinimum(0);
BleedRight->setMaximum(3000*unitRatio);
BleedTop->setNewUnit(unit);
BleedTop->setMinimum(0);
BleedTop->setMaximum(3000*unitRatio);
previewButton->setEnabled(!previewDinUse);
// Fill printer list
QString Pcap;
QString printerName;
QStringList printerNames = PrinterUtil::getPrinterNames();
int numPrinters = printerNames.count();
for( int i = 0; i < numPrinters; i++)
{
printerName = printerNames[i];
PrintDest->addItem(printerName);
if( printerName == printOptions.printer )
{
PrintDest->setCurrentIndex(PrintDest->count()-1);
prefs->set("CurrentPrn", PrintDest->currentText());
}
}
PrintDest->addItem( tr("File"));
// Fill Separation list
QString sep[] =
{
tr("All"), tr("Cyan"), tr("Magenta"), tr("Yellow"),
tr("Black")
};
size_t sepArray = sizeof(sep) / sizeof(*sep);
for (uint prop = 0; prop < sepArray; ++prop)
SepArt->addItem(sep[prop]);
SepArt->addItems(spots);
if (m_doc->pagePositioning() != 0)
{
BleedTxt3->setText( tr( "Inside:" ) );
BleedTxt4->setText( tr( "Outside:" ) );
}
QString prnDevice = printOptions.printer;
if (prnDevice.isEmpty())
prnDevice = PrintDest->currentText();
if ((prnDevice == tr("File")) || (PrintDest->count() == 1))
{
PrintDest->setCurrentIndex(PrintDest->count()-1);
prefs->set("CurrentPrn", PrintDest->currentText());
DateiT->setEnabled(true);
LineEdit1->setEnabled(true);
if (!printOptions.filename.isEmpty())
LineEdit1->setText(QDir::toNativeSeparators(printOptions.filename));
ToolButton1->setEnabled(true);
}
setMaximumSize(sizeHint());
PrintDest->setFocus();
// signals and slots connections
connect( OKButton, SIGNAL( clicked() ), this, SLOT( okButtonClicked() ) );
connect( OKButton_2, SIGNAL( clicked() ), this, SLOT( reject() ) );
connect( PrintDest, SIGNAL(activated(const QString&)), this, SLOT(SelPrinter(const QString&)));
connect( printEngines, SIGNAL(activated(const QString&)), this, SLOT(SelEngine(const QString&)));
connect( RadioButton1, SIGNAL(toggled(bool)), this, SLOT(SelRange(bool)));
connect( CurrentPage, SIGNAL(toggled(bool)), this, SLOT(SelRange(bool)));
connect( pageNrButton, SIGNAL(clicked()), this, SLOT(createPageNumberRange()));
connect( PrintSep, SIGNAL(activated(int)), this, SLOT(SelMode(int)));
connect( ToolButton1, SIGNAL(clicked()), this, SLOT(SelFile()));
connect( OtherCom, SIGNAL(clicked()), this, SLOT(SelComm()));
connect( previewButton, SIGNAL(clicked()), this, SLOT(previewButtonClicked()));
connect( docBleeds, SIGNAL(clicked()), this, SLOT(doDocBleeds()));
connect( OptButton, SIGNAL( clicked() ), this, SLOT( SetOptions() ) );
setStoredValues(printOptions.filename, gcr);
#if defined(_WIN32)
if (!outputToFile())
PrinterUtil::initDeviceSettings( PrintDest->currentText(), DevMode );
#endif
printEngineMap = PrinterUtil::getPrintEngineSupport(PrintDest->currentText(), outputToFile());
refreshPrintEngineBox();
bool ps1Supported = printEngineMap.contains(CommonStrings::trPostScript1);
bool ps2Supported = printEngineMap.contains(CommonStrings::trPostScript2);
bool ps3Supported = printEngineMap.contains(CommonStrings::trPostScript3);
bool psSupported = (ps1Supported || ps2Supported || ps3Supported);
printEngines->setEnabled(psSupported || outputToFile());
UseICC->setEnabled(m_doc->HasCMS && psSupported);
}
void MotionPlanningFrame::computePlanGoalsButtonClicked()
{
if (!move_group_)
return;
// Clear status
ui_->result_label->setText("Planning...");
// Reset the current plan.
current_plan_.reset(new moveit::planning_interface::MoveGroup::Plan());
// The primitive plan is used for actual execution on the robot. HACK
// We reset it here.
primitive_plan_.reset(new apc_msgs::PrimitivePlan);
// Get the list of goals (waypoints) to follow.
QListWidget* goals_list = ui_->active_goals_list;
// Get the current start state.
robot_state::RobotState start_state = *planning_display_->getQueryStartState();
// The target goal state will be initialized to the start state.
robot_state::RobotState goal_state = start_state;
// For each item in the active goals list, configure for planning and then
// append to the plan.
for (int i = 0; i < goals_list->count(); i++)
{
// First get the plan represented by the item.
apc_msgs::PrimitivePlan plan =
getMessageFromUserData<apc_msgs::PrimitivePlan>(goals_list->item(i)->data(Qt::UserRole));
// Loop through the actions in the plan.
for (int j = 0; j < plan.actions.size(); j++)
{
// Get the last action from the user data because it is the goal state.
apc_msgs::PrimitiveAction action = plan.actions.back();
// Get the goal robot state from user data.
getStateFromAction(goal_state, action);
// HACK Reset move group so that I can plan with a different group... SMH.
changePlanningGroupHelper(action.group_name);
planning_display_->waitForAllMainLoopJobs(); // I hope there are no cyclic main job loops.
// Set move group variables, like start and goal states, etc.
configureForPlanning(start_state, goal_state);
// Compute plan specific to the current primitive action.
moveit::planning_interface::MoveGroup::Plan move_group_plan;
// Make a planning service call. This will append any plans to the input.
if (!move_group_->plan(move_group_plan))
{
ui_->result_label->setText("Failed");
current_plan_.reset();
return;
}
// Copy plan over to primitive action.
action.joint_trajectory = move_group_plan.trajectory_.joint_trajectory;
action.duration = move_group_plan.planning_time_;
// TODO Eef trajectory support.
// TODO Dense trajectory support.
// HACK If we are planning for the hands, overwrite plan with linear interpolation.
if (action.group_name == "crichton_left_hand" ||
action.group_name == "crichton_right_hand")
{
computeLinearInterpPlan(start_state, action);
move_group_plan.trajectory_.joint_trajectory = action.joint_trajectory;
}
// Append plan onto the current plan.
move_group_->appendPlan(*current_plan_, move_group_plan);
// Append action onto primitive plan.
primitive_plan_->actions.push_back(action);
// Start the next plan from this goal.
start_state = goal_state;
}
}
// Success
ui_->execute_button->setEnabled(true);
ui_->result_label->setText(QString("Time: ").append(
QString::number(current_plan_->planning_time_,'f',3)));
// Copy trajectory over to display.
{
// Get a robot model.
const robot_model::RobotModelConstPtr& robot_model = planning_display_->getRobotModel();
// Construct a new robot trajectory.
robot_trajectory::RobotTrajectoryPtr display_trajectory(new robot_trajectory::RobotTrajectory(robot_model, ""));
// Copy current plan over to robot trajectory.
display_trajectory->setRobotTrajectoryMsg(planning_display_->getPlanningSceneRO()->getCurrentState(),
current_plan_->start_state_,
current_plan_->trajectory_);
// Swap the plan trajectory into our planning display.
planning_display_->setTrajectoryToDisplay(display_trajectory);
// Display trail. FIXME This doesn't accomplish anything actually.
previewButtonClicked();
}
}
