trajectory.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, 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 */
00036 
00037 #include <trajectory.h>
00038 #include <job_processing.h>
00039 
00040 #include <eigen_conversions/eigen_msg.h>
00041 
00042 #include <boost/math/constants/constants.hpp>
00043 
00044 #include <QWidget>
00045 
00046 namespace benchmark_tool
00047 {
00048 const std::string Trajectory::TRAJECTORY_SET_START_POSE_STRING = "Set start pose";
00049 const std::string Trajectory::TRAJECTORY_SET_END_POSE_STRING = "Set end pose";
00050 const std::string Trajectory::TRAJECTORY_EDIT_CONTROL_FRAME_STRING = "Edit control frame";
00051 const std::string Trajectory::TRAJECTORY_FIX_CONTROL_FRAME_STRING = "Fix control frame";
00052 
00053 Trajectory::Trajectory(const robot_state::RobotState& robot_state, Ogre::SceneNode* parent_node,
00054                        rviz::DisplayContext* context, const std::string& name, const std::string& frame_id,
00055                        const robot_interaction::RobotInteraction::EndEffector& eef, const geometry_msgs::Pose& pose,
00056                        double scale, const GripperMarker::GripperMarkerState& state, unsigned int nwaypoints,
00057                        bool is_selected, bool visible_x, bool visible_y, bool visible_z)
00058   : dragging_(false), nwaypoints_(nwaypoints), control_marker_mode_(CONTROL_MARKER_FIXED)
00059 {
00060   createControlMarker(robot_state, parent_node, context, name, frame_id, eef, pose, scale, state, is_selected,
00061                       visible_x, visible_y, visible_z);
00062   createHandMarker();
00063 }
00064 
00065 void Trajectory::createControlMarker(const robot_state::RobotState& robot_state, Ogre::SceneNode* parent_node,
00066                                      rviz::DisplayContext* context, const std::string& name,
00067                                      const std::string& frame_id,
00068                                      const robot_interaction::RobotInteraction::EndEffector& eef,
00069                                      const geometry_msgs::Pose& pose, double scale,
00070                                      const GripperMarker::GripperMarkerState& state, bool is_selected, bool visible_x,
00071                                      bool visible_y, bool visible_z)
00072 {
00073   GripperMarkerPtr control(
00074       new GripperMarker(robot_state, parent_node, context, name, frame_id, eef, pose, scale, state));
00075   control->select(true);
00076   std::vector<visualization_msgs::MenuEntry> menu_entries;
00077   visualization_msgs::MenuEntry m;
00078   m.id = TRAJECTORY_SET_START_POSE;
00079   m.command_type = m.FEEDBACK;
00080   m.parent_id = 0;
00081   m.title = TRAJECTORY_SET_START_POSE_STRING;
00082   menu_entries.push_back(m);
00083   m.id = TRAJECTORY_SET_END_POSE;
00084   m.command_type = m.FEEDBACK;
00085   m.parent_id = 0;
00086   m.title = TRAJECTORY_SET_END_POSE_STRING;
00087   menu_entries.push_back(m);
00088   m.id = TRAJECTORY_EDIT_CONTROL_FRAME;
00089   m.command_type = m.FEEDBACK;
00090   m.parent_id = 0;
00091   m.title = TRAJECTORY_EDIT_CONTROL_FRAME_STRING;
00092   menu_entries.push_back(m);
00093   control->setMenu(menu_entries);
00094   control->select(false);
00095 
00096   control_marker = control;
00097   connectControlMarker();
00098 }
00099 
00100 void Trajectory::createHandMarker()
00101 {
00102   if (control_marker)
00103   {
00104     hand_marker = GripperMarkerPtr(new GripperMarker(*control_marker));
00105     hand_marker->unselect(true);
00106     connectHandMarker();
00107   }
00108   else
00109   {
00110     ROS_ERROR("Main trajectory marker does not exist");
00111   }
00112 }
00113 
00114 void Trajectory::createStartMarker()
00115 {
00116   if (control_marker)
00117   {
00118     start_marker = GripperMarkerPtr(new GripperMarker(*hand_marker));
00119     start_marker->unselect(true);
00120     start_marker->setColor(0.0, 0.9, 0.0, 1.0);
00121     start_marker->showDescription("Start");
00122     connectStartMarker();
00123   }
00124   else
00125   {
00126     ROS_ERROR("Main trajectory marker does not exist");
00127   }
00128 }
00129 
00130 void Trajectory::createEndMarker()
00131 {
00132   if (control_marker)
00133   {
00134     end_marker = GripperMarkerPtr(new GripperMarker(*hand_marker));
00135     end_marker->unselect(true);
00136     end_marker->setColor(0.0, 0.9, 0.0, 0.5);
00137     end_marker->showDescription("End");
00138     connectEndMarker();
00139   }
00140   else
00141   {
00142     ROS_ERROR("Main trajectory marker does not exist");
00143   }
00144 }
00145 
00146 void Trajectory::connectControlMarker()
00147 {
00148   control_marker->connect(this, SLOT(trajectoryMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
00149 }
00150 
00151 void Trajectory::connectHandMarker()
00152 {
00153   hand_marker->connect(this, SLOT(handMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
00154 }
00155 
00156 void Trajectory::connectStartMarker()
00157 {
00158   start_marker->connect(this, SLOT(startMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
00159 }
00160 void Trajectory::connectEndMarker()
00161 {
00162   end_marker->connect(this, SLOT(endMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
00163 }
00164 
00165 void Trajectory::rebuildWayPointMarkers()
00166 {
00167   waypoint_markers.clear();
00168   if (start_marker && end_marker)
00169   {
00170     Eigen::Vector3d start_t(control_marker_start_pose.translation());
00171     Eigen::Vector3d end_t(control_marker_end_pose.translation());
00172 
00173     Eigen::Quaterniond start_r(control_marker_start_pose.rotation());
00174     Eigen::Quaterniond end_r(control_marker_end_pose.rotation());
00175 
00176     Eigen::Affine3d wMh;
00177     hand_marker->getPose(wMh);
00178 
00179     Eigen::Affine3d wMt;
00180     control_marker->getPose(wMt);
00181 
00182     Eigen::Affine3d tMh = wMt.inverse() * wMh;
00183 
00184     unsigned int nfragments = nwaypoints_ + 1;
00185     for (std::size_t i = 0; i <= nfragments; ++i)
00186     {
00187       Eigen::Vector3d waypoint_t = ((nfragments - i) * start_t + i * end_t) / nfragments;
00188       Eigen::Quaterniond waypoint_r = start_r.slerp((double)i / (double)nfragments, end_r);
00189       Eigen::Affine3d wMwpt(waypoint_r);
00190       wMwpt.translation() = waypoint_t;
00191 
00192       Eigen::Affine3d wMwph = wMwpt * tMh;
00193 
00194       GripperMarkerPtr waypoint(new GripperMarker(*hand_marker));
00195       std::stringstream name;
00196       name << hand_marker->getName() << "_wp" << i;
00197       waypoint->setName(name.str());
00198       waypoint->unselect(true);
00199       waypoint->setColor(0.0, 0.9, 0.0, 1 - (double)i / (double)nfragments);
00200       Eigen::Quaterniond rotation(wMwph.rotation());
00201       waypoint->imarker->setPose(Ogre::Vector3(wMwph(0, 3), wMwph(1, 3), wMwph(2, 3)),
00202                                  Ogre::Quaternion(rotation.w(), rotation.x(), rotation.y(), rotation.z()), "");
00203       waypoint_markers.push_back(waypoint);
00204     }
00205   }
00206 }
00207 
00208 void Trajectory::trajectoryMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback)
00209 {
00210   if (feedback.event_type == feedback.MENU_SELECT)
00211   {
00212     if (feedback.menu_entry_id == TRAJECTORY_SET_START_POSE)
00213     {
00214       if (control_marker_mode_ == CONTROL_MARKER_FLOATING)
00215       {
00216         // If the control marker is not fixed, fix it
00217         fixControlFrame();
00218       }
00219 
00220       control_marker->getPose(control_marker_start_pose);
00221 
00222       // Create start marker
00223       JobProcessing::addMainLoopJob(boost::bind(&benchmark_tool::Trajectory::createStartMarker, this));
00224       JobProcessing::addMainLoopJob(boost::bind(&benchmark_tool::Trajectory::rebuildWayPointMarkers, this));
00225     }
00226     else if (feedback.menu_entry_id == TRAJECTORY_SET_END_POSE)
00227     {
00228       if (control_marker_mode_ == CONTROL_MARKER_FLOATING)
00229       {
00230         // If the control marker is not fixed, fix it
00231         fixControlFrame();
00232       }
00233 
00234       control_marker->getPose(control_marker_end_pose);
00235 
00236       // Create end marker
00237       JobProcessing::addMainLoopJob(boost::bind(&benchmark_tool::Trajectory::createEndMarker, this));
00238       JobProcessing::addMainLoopJob(boost::bind(&benchmark_tool::Trajectory::rebuildWayPointMarkers, this));
00239     }
00240     else if (feedback.menu_entry_id == TRAJECTORY_EDIT_CONTROL_FRAME)
00241     {
00242       editControlFrame();
00243     }
00244     else if (feedback.menu_entry_id == TRAJECTORY_FIX_CONTROL_FRAME)
00245     {
00246       fixControlFrame();
00247     }
00248   }
00249   else if (feedback.event_type == feedback.MOUSE_DOWN && control_marker_mode_ == CONTROL_MARKER_FIXED)
00250   {
00251     // Store current hand pose
00252     hand_marker->getPose(hand_marker_start_pose);
00253     control_marker->getPose(control_marker_drag_start_pose);
00254 
00255     dragging_ = true;
00256   }
00257   else if (feedback.event_type == feedback.POSE_UPDATE && dragging_ && control_marker_mode_ == CONTROL_MARKER_FIXED)
00258   {
00259     // Compute displacement from stored pose, and apply to the rest of selected markers
00260     Eigen::Affine3d current_pose_eigen;
00261     tf::poseMsgToEigen(feedback.pose, current_pose_eigen);
00262 
00263     Eigen::Affine3d current_wrt_initial = control_marker_drag_start_pose.inverse() * current_pose_eigen;
00264 
00265     visualization_msgs::InteractiveMarkerPose impose;
00266     Eigen::Affine3d newpose = control_marker_drag_start_pose * current_wrt_initial *
00267                               control_marker_drag_start_pose.inverse() * hand_marker_start_pose;
00268     tf::poseEigenToMsg(newpose, impose.pose);
00269 
00270     hand_marker->imarker->setPose(Ogre::Vector3(impose.pose.position.x, impose.pose.position.y, impose.pose.position.z),
00271                                   Ogre::Quaternion(impose.pose.orientation.w, impose.pose.orientation.x,
00272                                                    impose.pose.orientation.y, impose.pose.orientation.z),
00273                                   "");
00274   }
00275   else if (feedback.event_type == feedback.MOUSE_UP)
00276   {
00277     dragging_ = false;
00278   }
00279 }
00280 
00281 void Trajectory::editControlFrame()
00282 {
00283   start_marker.reset();
00284   end_marker.reset();
00285   waypoint_markers.clear();
00286 
00287   control_marker_mode_ = CONTROL_MARKER_FLOATING;
00288 
00289   std::vector<visualization_msgs::MenuEntry> menu_entries;
00290   visualization_msgs::MenuEntry m;
00291   m.id = TRAJECTORY_SET_START_POSE;
00292   m.command_type = m.FEEDBACK;
00293   m.parent_id = 0;
00294   m.title = TRAJECTORY_SET_START_POSE_STRING;
00295   menu_entries.push_back(m);
00296   m.id = TRAJECTORY_SET_END_POSE;
00297   m.command_type = m.FEEDBACK;
00298   m.parent_id = 0;
00299   m.title = TRAJECTORY_SET_END_POSE_STRING;
00300   menu_entries.push_back(m);
00301   m.id = TRAJECTORY_FIX_CONTROL_FRAME;
00302   m.command_type = m.FEEDBACK;
00303   m.parent_id = 0;
00304   m.title = TRAJECTORY_FIX_CONTROL_FRAME_STRING;
00305   menu_entries.push_back(m);
00306 
00307   JobProcessing::addMainLoopJob(boost::bind(&benchmark_tool::GripperMarker::setMenu, control_marker, menu_entries));
00308   JobProcessing::addMainLoopJob(boost::bind(&benchmark_tool::Trajectory::connectControlMarker, this));
00309 }
00310 
00311 void Trajectory::fixControlFrame()
00312 {
00313   control_marker_mode_ = CONTROL_MARKER_FIXED;
00314 
00315   std::vector<visualization_msgs::MenuEntry> menu_entries;
00316   visualization_msgs::MenuEntry m;
00317   m.id = TRAJECTORY_SET_START_POSE;
00318   m.command_type = m.FEEDBACK;
00319   m.parent_id = 0;
00320   m.title = TRAJECTORY_SET_START_POSE_STRING;
00321   menu_entries.push_back(m);
00322   m.id = TRAJECTORY_SET_END_POSE;
00323   m.command_type = m.FEEDBACK;
00324   m.parent_id = 0;
00325   m.title = TRAJECTORY_SET_END_POSE_STRING;
00326   menu_entries.push_back(m);
00327   m.id = TRAJECTORY_EDIT_CONTROL_FRAME;
00328   m.command_type = m.FEEDBACK;
00329   m.parent_id = 0;
00330   m.title = TRAJECTORY_EDIT_CONTROL_FRAME_STRING;
00331   menu_entries.push_back(m);
00332 
00333   JobProcessing::addMainLoopJob(boost::bind(&benchmark_tool::GripperMarker::setMenu, control_marker, menu_entries));
00334   JobProcessing::addMainLoopJob(boost::bind(&benchmark_tool::Trajectory::connectControlMarker, this));
00335 }
00336 
00337 void Trajectory::startMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback)
00338 {
00339 }
00340 
00341 void Trajectory::endMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback)
00342 {
00343 }
00344 
00345 void Trajectory::handMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback)
00346 {
00347 }
00348 
00349 }  // namespace


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