trajectory.h
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 #ifndef BT_TRAJECTORY
00038 #define BT_TRAJECTORY
00039 
00040 #ifndef Q_MOC_RUN
00041 #include <frame_marker.h>
00042 #include <moveit/macros/class_forward.h>
00043 #endif
00044 
00045 namespace benchmark_tool
00046 {
00047 MOVEIT_CLASS_FORWARD(Trajectory);
00048 
00049 /* Implements functionality to create and manage straight and arc trajectories with interactive markers
00050  */
00051 class Trajectory : public QObject
00052 {
00053   Q_OBJECT
00054 
00055 public Q_SLOTS:
00056   void trajectoryMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback);
00057   void handMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback);
00058   void startMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback);
00059   void endMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback);
00060 
00061 public:
00062   static const int TRAJECTORY_SET_START_POSE = 1;
00063   static const int TRAJECTORY_SET_END_POSE = 2;
00064   static const int TRAJECTORY_EDIT_CONTROL_FRAME = 3;
00065   static const int TRAJECTORY_FIX_CONTROL_FRAME = 4;
00066 
00067   static const std::string TRAJECTORY_SET_START_POSE_STRING;
00068   static const std::string TRAJECTORY_SET_END_POSE_STRING;
00069   static const std::string TRAJECTORY_EDIT_CONTROL_FRAME_STRING;
00070   static const std::string TRAJECTORY_FIX_CONTROL_FRAME_STRING;
00071 
00072   GripperMarkerPtr control_marker;
00073   GripperMarkerPtr hand_marker;
00074   GripperMarkerPtr start_marker, end_marker;
00075   std::vector<GripperMarkerPtr> waypoint_markers;
00076 
00077   Eigen::Affine3d control_marker_start_pose;  // The control marker pose corresponding to the start marker
00078   Eigen::Affine3d control_marker_end_pose;    // The control marker pose corresponding to the end marker
00079 
00080   Trajectory(const robot_state::RobotState& robot_state, Ogre::SceneNode* parent_node, rviz::DisplayContext* context,
00081              const std::string& name, const std::string& frame_id,
00082              const robot_interaction::RobotInteraction::EndEffector& eef, const geometry_msgs::Pose& pose, double scale,
00083              const GripperMarker::GripperMarkerState& state, unsigned int nwaypoints, bool is_selected = true,
00084              bool visible_x = true, bool visible_y = true, bool visible_z = true);
00085 
00086   void setControlMarker(const GripperMarkerPtr control_marker);
00087 
00088   void setNumberOfWaypoints(unsigned int n)
00089   {
00090     nwaypoints_ = n;
00091     if (waypoint_markers.size())
00092     {
00093       rebuildWayPointMarkers();
00094     }
00095   }
00096 
00097   void hide()
00098   {
00099     if (control_marker)
00100       control_marker->hide();
00101     if (hand_marker)
00102       hand_marker->hide();
00103     if (start_marker)
00104       start_marker->hide();
00105     if (end_marker)
00106       end_marker->hide();
00107     for (std::vector<GripperMarkerPtr>::iterator it = waypoint_markers.begin(); it != waypoint_markers.end(); ++it)
00108     {
00109       (*it)->hide();
00110     }
00111   }
00112 
00113   void show(Ogre::SceneNode* scene_node, rviz::DisplayContext* context)
00114   {
00115     if (control_marker)
00116       control_marker->show(scene_node, context);
00117     if (hand_marker)
00118       hand_marker->show(scene_node, context);
00119     if (start_marker)
00120       start_marker->show(scene_node, context);
00121     if (end_marker)
00122       end_marker->show(scene_node, context);
00123     for (std::vector<GripperMarkerPtr>::iterator it = waypoint_markers.begin(); it != waypoint_markers.end(); ++it)
00124     {
00125       (*it)->show(scene_node, context);
00126     }
00127   }
00128 
00129   ~Trajectory()
00130   {
00131   }
00132 
00133 protected:
00134   void createControlMarker(const robot_state::RobotState& robot_state, Ogre::SceneNode* parent_node,
00135                            rviz::DisplayContext* context, const std::string& name, const std::string& frame_id,
00136                            const robot_interaction::RobotInteraction::EndEffector& eef, const geometry_msgs::Pose& pose,
00137                            double scale, const GripperMarker::GripperMarkerState& state, bool is_selected = true,
00138                            bool visible_x = true, bool visible_y = true, bool visible_z = true);
00139 
00140   void createHandMarker();
00141   void createStartMarker();
00142   void createEndMarker();
00143 
00144   void connectControlMarker();
00145   void connectHandMarker();
00146   void connectStartMarker();
00147   void connectEndMarker();
00148 
00149   void rebuildWayPointMarkers();
00150 
00151 private:
00152   void editControlFrame();
00153   void fixControlFrame();
00154 
00155   Eigen::Affine3d hand_marker_start_pose;
00156   Eigen::Affine3d control_marker_drag_start_pose;
00157 
00158   bool dragging_;
00159 
00160   unsigned int nwaypoints_;
00161 
00162   typedef enum { CONTROL_MARKER_FLOATING, CONTROL_MARKER_FIXED } ControlMarkerModeType;
00163   ControlMarkerModeType control_marker_mode_;
00164 };
00165 }
00166 
00167 #endif


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