tab_trajectories.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Mario Prats, Ioan Sucan */
00036 
00037 #include <main_window.h>
00038 #include <trajectory.h>
00039 #include <job_processing.h>
00040 #include <ui_utils.h>
00041 
00042 #include <eigen_conversions/eigen_msg.h>
00043 
00044 #include <QMessageBox>
00045 #include <QInputDialog>
00046 
00047 #include <boost/math/constants/constants.hpp>
00048 
00049 namespace benchmark_tool
00050 {
00051 void MainWindow::createTrajectoryButtonClicked(void)
00052 {
00053   std::stringstream ss;
00054 
00055   {
00056     const planning_scene_monitor::LockedPlanningSceneRO& ps = scene_display_->getPlanningSceneRO();
00057     if (!ps || robot_interaction_->getActiveEndEffectors().empty())
00058     {
00059       if (!ps)
00060         ROS_ERROR("Not planning scene");
00061       if (robot_interaction_->getActiveEndEffectors().empty())
00062         ROS_ERROR("No end effector");
00063       return;
00064     }
00065     else
00066       ss << ps->getName().c_str() << "_trajectory_" << std::setfill('0') << std::setw(4) << trajectories_.size();
00067   }
00068 
00069   bool ok = false;
00070   QString text = QInputDialog::getText(this, tr("Choose a name"), tr("Trajectory name:"), QLineEdit::Normal,
00071                                        QString(ss.str().c_str()), &ok);
00072 
00073   std::string name;
00074   if (ok)
00075   {
00076     if (!text.isEmpty())
00077     {
00078       name = text.toStdString();
00079       if (goal_poses_.find(name) != goal_poses_.end())
00080         QMessageBox::warning(
00081             this, "Name already exists",
00082             QString("The name '").append(name.c_str()).append("' already exists. Not creating trajectory."));
00083       else
00084       {
00085         // Create the new trajectory starting point at the current eef pose, and attach an interactive marker to it
00086         scene_display_->getPlanningSceneRW()->getCurrentStateNonConst().update();
00087         Eigen::Affine3d tip_pose = scene_display_->getPlanningSceneRO()->getCurrentState().getGlobalLinkTransform(
00088             robot_interaction_->getActiveEndEffectors()[0].parent_link);
00089         geometry_msgs::Pose marker_pose;
00090         tf::poseEigenToMsg(tip_pose, marker_pose);
00091         static const float marker_scale = 0.15;
00092 
00093         TrajectoryPtr trajectory_marker(
00094             new Trajectory(scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(),
00095                            visualization_manager_, name, scene_display_->getRobotModel()->getModelFrame(),
00096                            robot_interaction_->getActiveEndEffectors()[0], marker_pose, marker_scale,
00097                            GripperMarker::NOT_TESTED, ui_.trajectory_nwaypoints_spin->value()));
00098 
00099         trajectories_.insert(TrajectoryPair(name, trajectory_marker));
00100       }
00101     }
00102     else
00103       QMessageBox::warning(this, "Goal not created", "Cannot use an empty name for a new goal pose.");
00104   }
00105 
00106   populateTrajectoriesList();
00107   selectLastItemInList(ui_.trajectory_list);
00108 }
00109 
00110 void MainWindow::populateTrajectoriesList(void)
00111 {
00112   bool old_signal_state = ui_.trajectory_list->blockSignals(true);
00113   ui_.trajectory_list->clear();
00114   for (TrajectoryMap::iterator it = trajectories_.begin(); it != trajectories_.end(); ++it)
00115   {
00116     QListWidgetItem* item = new QListWidgetItem(QString(it->first.c_str()));
00117     ui_.trajectory_list->addItem(item);
00118   }
00119   ui_.trajectory_list->blockSignals(old_signal_state);
00120 }
00121 
00122 void MainWindow::trajectorySelectionChanged(void)
00123 {
00124   for (unsigned int i = 0; i < ui_.trajectory_list->count(); ++i)
00125   {
00126     QListWidgetItem* item = ui_.trajectory_list->item(i);
00127     std::string name = item->text().toStdString();
00128     if (trajectories_.find(name) != trajectories_.end() && item->isSelected())
00129     {
00130       trajectories_[name]->show(scene_display_->getSceneNode(), visualization_manager_);
00131     }
00132     else
00133     {
00134       trajectories_[name]->hide();
00135     }
00136   }
00137 }
00138 
00139 void MainWindow::removeTrajectoryButtonClicked(void)
00140 {
00141   if (ui_.trajectory_list->currentItem())
00142   {
00143     // Warn the user
00144     QMessageBox msgBox;
00145     msgBox.setText("The selected trajectory will be removed from the database");
00146     msgBox.setInformativeText("Do you want to continue?");
00147     msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
00148     msgBox.setDefaultButton(QMessageBox::No);
00149     int ret = msgBox.exec();
00150 
00151     switch (ret)
00152     {
00153       case QMessageBox::Yes:
00154       {
00155         // Go through the list of trajectories, and delete those selected
00156         QList<QListWidgetItem*> found_items = ui_.trajectory_list->selectedItems();
00157         for (std::size_t i = 0; i < found_items.size(); i++)
00158         {
00159           try
00160           {
00161             trajectory_constraints_storage_->removeTrajectoryConstraints(found_items[i]->text().toStdString());
00162           }
00163           catch (std::runtime_error& ex)
00164           {
00165             ROS_ERROR("%s", ex.what());
00166           }
00167           trajectories_.erase(found_items[i]->text().toStdString());
00168         }
00169         populateTrajectoriesList();
00170       }
00171       break;
00172     }
00173   }
00174 }
00175 
00176 void MainWindow::loadTrajectoriesFromDBButtonClicked(void)
00177 {
00178   // Get all the trajectory constraints from the database
00179   if (trajectory_constraints_storage_ && !robot_interaction_->getActiveEndEffectors().empty())
00180   {
00181     // First clear the current list
00182     trajectories_.clear();
00183 
00184     std::vector<std::string> names;
00185     try
00186     {
00187       trajectory_constraints_storage_->getKnownTrajectoryConstraints(ui_.trajectories_filter_text->text().toStdString(),
00188                                                                      names);
00189     }
00190     catch (std::runtime_error& ex)
00191     {
00192       QMessageBox::warning(this, "Cannot query the database", QString("Wrongly formatted regular expression for "
00193                                                                       "trajectory contraints: ")
00194                                                                   .append(ex.what()));
00195       return;
00196     }
00197 
00198     for (unsigned int i = 0; i < names.size(); i++)
00199     {
00200       // Create a trajectory constraint
00201       moveit_warehouse::TrajectoryConstraintsWithMetadata tc;
00202       bool got_constraint = false;
00203       try
00204       {
00205         got_constraint = trajectory_constraints_storage_->getTrajectoryConstraints(tc, names[i]);
00206       }
00207       catch (std::runtime_error& ex)
00208       {
00209         ROS_ERROR("%s", ex.what());
00210       }
00211       if (!got_constraint)
00212         continue;
00213 
00214       if (tc->constraints.size() > 0 &&
00215           tc->constraints[0].position_constraints[0].constraint_region.primitive_poses.size() > 0 &&
00216           tc->constraints[0].orientation_constraints.size() > 0)
00217       {
00218         geometry_msgs::Pose shape_pose;
00219         shape_pose.position = tc->constraints[0].position_constraints[0].constraint_region.primitive_poses[0].position;
00220         shape_pose.orientation = tc->constraints[0].orientation_constraints[0].orientation;
00221         static const float marker_scale = 0.15;
00222         TrajectoryPtr trajectory_marker(
00223             new Trajectory(scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(),
00224                            visualization_manager_, names[i], scene_display_->getRobotModel()->getModelFrame(),
00225                            robot_interaction_->getActiveEndEffectors()[0], shape_pose, marker_scale,
00226                            GripperMarker::NOT_TESTED, ui_.trajectory_nwaypoints_spin->value()));
00227 
00228         if (trajectories_.find(names[i]) != trajectories_.end())
00229         {
00230           trajectories_.erase(names[i]);
00231         }
00232         trajectories_.insert(TrajectoryPair(names[i], trajectory_marker));
00233 
00234         for (std::size_t c = 0; c < tc->constraints.size(); ++c)
00235         {
00236           if (tc->constraints[c].position_constraints.size() > 0 &&
00237               tc->constraints[c].position_constraints[0].constraint_region.primitive_poses.size() > 0 &&
00238               tc->constraints[c].orientation_constraints.size() > 0)
00239           {
00240             shape_pose.position =
00241                 tc->constraints[c].position_constraints[0].constraint_region.primitive_poses[0].position;
00242             shape_pose.orientation = tc->constraints[c].orientation_constraints[0].orientation;
00243 
00244             static const float marker_scale = 0.15;
00245             GripperMarkerPtr waypoint_marker(new GripperMarker(
00246                 scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(),
00247                 visualization_manager_, names[i], scene_display_->getRobotModel()->getModelFrame(),
00248                 robot_interaction_->getActiveEndEffectors()[0], shape_pose, marker_scale, GripperMarker::NOT_TESTED));
00249             waypoint_marker->unselect(true);
00250             waypoint_marker->setColor(0.0, 0.9, 0.0, 1 - (double)c / (double)tc->constraints.size());
00251             trajectory_marker->waypoint_markers.push_back(waypoint_marker);
00252           }
00253         }
00254 
00255         if (trajectory_marker->waypoint_markers.size() > 0)
00256         {
00257           trajectory_marker->start_marker =
00258               GripperMarkerPtr(new GripperMarker(*trajectory_marker->waypoint_markers.front()));
00259           trajectory_marker->end_marker =
00260               GripperMarkerPtr(new GripperMarker(*trajectory_marker->waypoint_markers.back()));
00261           trajectory_marker->waypoint_markers.front()->getPose(trajectory_marker->control_marker_start_pose);
00262           trajectory_marker->waypoint_markers.back()->getPose(trajectory_marker->control_marker_end_pose);
00263         }
00264         populateTrajectoriesList();
00265         selectFirstItemInList(ui_.trajectory_list);
00266       }
00267     }
00268   }
00269   else
00270   {
00271     if (!trajectory_constraints_storage_)
00272       QMessageBox::warning(this, "Warning", "Not connected to a database.");
00273   }
00274 }
00275 
00276 void MainWindow::saveTrajectoriesOnDBButtonClicked(void)
00277 {
00278   if (trajectory_constraints_storage_)
00279   {
00280     // Convert all goal trajectory markers into constraints and store them
00281     for (TrajectoryMap::iterator it = trajectories_.begin(); it != trajectories_.end(); ++it)
00282     {
00283       moveit_msgs::TrajectoryConstraints tc;
00284 
00285       for (std::size_t w = 0; w < it->second->waypoint_markers.size(); ++w)
00286       {
00287         moveit_msgs::Constraints c;
00288         c.name = it->second->waypoint_markers[w]->getName();
00289 
00290         shape_msgs::SolidPrimitive sp;
00291         sp.type = sp.BOX;
00292         sp.dimensions.resize(3, std::numeric_limits<float>::epsilon() * 10.0);
00293 
00294         moveit_msgs::PositionConstraint pc;
00295         pc.constraint_region.primitives.push_back(sp);
00296         geometry_msgs::Pose posemsg;
00297         it->second->waypoint_markers[w]->getPosition(posemsg.position);
00298         posemsg.orientation.x = 0.0;
00299         posemsg.orientation.y = 0.0;
00300         posemsg.orientation.z = 0.0;
00301         posemsg.orientation.w = 1.0;
00302         pc.constraint_region.primitive_poses.push_back(posemsg);
00303         pc.weight = 1.0;
00304         c.position_constraints.push_back(pc);
00305 
00306         moveit_msgs::OrientationConstraint oc;
00307         it->second->waypoint_markers[w]->getOrientation(oc.orientation);
00308         oc.absolute_x_axis_tolerance = oc.absolute_y_axis_tolerance = oc.absolute_z_axis_tolerance =
00309             std::numeric_limits<float>::epsilon() * 10.0;
00310         oc.weight = 1.0;
00311         c.orientation_constraints.push_back(oc);
00312         tc.constraints.push_back(c);
00313       }
00314 
00315       try
00316       {
00317         trajectory_constraints_storage_->addTrajectoryConstraints(tc, it->first);
00318       }
00319       catch (std::runtime_error& ex)
00320       {
00321         ROS_ERROR("Cannot save trajectory constraint: %s", ex.what());
00322       }
00323     }
00324   }
00325   else
00326   {
00327     QMessageBox::warning(this, "Warning", "Not connected to a database.");
00328   }
00329 }
00330 
00331 void MainWindow::trajectoryNWaypointsChanged(int n)
00332 {
00333   if (ui_.trajectory_list->currentItem())
00334   {
00335     TrajectoryMap::iterator it = trajectories_.find(ui_.trajectory_list->currentItem()->text().toStdString());
00336     it->second->setNumberOfWaypoints(n);
00337   }
00338 }
00339 
00340 void MainWindow::trajectoryExecuteButtonClicked()
00341 {
00342   if (ui_.trajectory_list->currentItem())
00343   {
00344     TrajectoryMap::iterator it = trajectories_.find(ui_.trajectory_list->currentItem()->text().toStdString());
00345     EigenSTL::vector_Affine3d waypoint_poses;
00346     for (std::size_t w = 1; w < it->second->waypoint_markers.size(); ++w)
00347     {
00348       Eigen::Affine3d pose;
00349       it->second->waypoint_markers[w]->getPose(pose);
00350       waypoint_poses.push_back(pose);
00351     }
00352 
00353     if (waypoint_poses.size() > 0)
00354     {
00355       robot_state::RobotState& rstate = scene_display_->getPlanningSceneRW()->getCurrentStateNonConst();
00356       rstate.update();
00357       const robot_model::JointModelGroup* jmg =
00358           rstate.getJointModelGroup(ui_.planning_group_combo->currentText().toStdString());
00359 
00360       std::vector<robot_state::RobotStatePtr> traj;
00361       double completed = rstate.computeCartesianPath(
00362           jmg, traj, rstate.getLinkModel(robot_interaction_->getActiveEndEffectors()[0].parent_link), waypoint_poses,
00363           true, 0.04, 0.0);
00364 
00365       ROS_INFO_STREAM("Trajectory completion percentage " << completed);
00366       JobProcessing::addBackgroundJob(boost::bind(&MainWindow::animateTrajectory, this, traj));
00367     }
00368   }
00369 }
00370 
00371 void MainWindow::animateTrajectory(const std::vector<robot_state::RobotStatePtr>& traj)
00372 {
00373   for (std::size_t i = 0; i < traj.size(); ++i)
00374   {
00375     scene_display_->getPlanningSceneRW()->setCurrentState(*traj[i]);
00376     scene_display_->queueRenderSceneGeometry();
00377     usleep(100000);
00378   }
00379 }
00380 
00381 }  // namespace


benchmarks_gui
Author(s): Mario Prats
autogenerated on Mon Jul 24 2017 02:22:21