frame_marker.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_FRAME_MARKER
00038 #define BT_FRAME_MARKER
00039 
00040 #include <rviz/display_context.h>
00041 #include <rviz/default_plugin/interactive_markers/interactive_marker.h>
00042 
00043 #include <moveit/robot_interaction/robot_interaction.h>
00044 #include <moveit/robot_interaction/interaction_handler.h>
00045 #include <moveit/robot_state/robot_state.h>
00046 
00047 #include <QObject>
00048 
00049 #include <string>
00050 
00051 namespace benchmark_tool
00052 {
00053 
00054 class MotionPlanningDisplay;
00055 
00060 class FrameMarker
00061 {
00062 public:
00063   visualization_msgs::InteractiveMarker imarker_msg;
00064   boost::shared_ptr<rviz::InteractiveMarker> imarker;
00065 
00067   FrameMarker(const FrameMarker &frame_marker): receiver_(NULL), receiver_method_(NULL)
00068   {
00069     imarker_msg = frame_marker.imarker_msg;
00070 
00071     visible_x_ = frame_marker.visible_x_;
00072     visible_y_ = frame_marker.visible_y_;
00073     visible_z_ = frame_marker.visible_z_;
00074 
00075     parent_node_ = frame_marker.parent_node_;
00076     context_ = frame_marker.context_;
00077 
00078     selected_ = frame_marker.selected_;
00079     color_ = frame_marker.color_;
00080 
00081     position_ = frame_marker.imarker->getPosition();
00082     orientation_ = frame_marker.imarker->getOrientation();
00083 
00084     rebuild();
00085   }
00086 
00098   FrameMarker(Ogre::SceneNode *parent_node, rviz::DisplayContext *context, const std::string &name,
00099               const std::string &frame_id, const geometry_msgs::Pose &pose, double scale, const std_msgs::ColorRGBA &color,
00100               bool is_selected = false, bool visible_x = true, bool visible_y = true, bool visible_z = true);
00101 
00102   FrameMarker(Ogre::SceneNode *parent_node, rviz::DisplayContext *context, const std::string &name,
00103               const std::string &frame_id, const geometry_msgs::Pose &pose, double scale, const float color[4],
00104               bool is_selected = false, bool visible_x = true, bool visible_y = true, bool visible_z = true);
00105 
00106   virtual void updateMarker(void)
00107   {
00108     imarker->processMessage(imarker_msg);
00109   }
00110 
00111   virtual void hide(void);
00112   virtual void show(Ogre::SceneNode *scene_node, rviz::DisplayContext *context);
00113 
00114   virtual void showDescription(const std::string &description);
00115   virtual void hideDescription();
00116 
00117   virtual void setAxisVisibility(bool x, bool y, bool z)
00118   {
00119     visible_x_ = x;
00120     visible_y_ = y;
00121     visible_z_ = z;
00122     rebuild();
00123   }
00124 
00125   virtual void setColor(float r, float g, float b, float a);
00126 
00127   virtual void getPosition(geometry_msgs::Point &position);
00128   virtual void getOrientation(geometry_msgs::Quaternion &orientation);
00129   virtual void getPose(Eigen::Affine3d &pose);
00130   virtual void setPose(Eigen::Affine3d &pose);
00131 
00132   virtual void select(void);
00133   virtual void unselect(void);
00134 
00135   virtual void setMenu(std::vector<visualization_msgs::MenuEntry> entries)
00136   {
00137     menu_entries_ = entries;
00138     rebuild();
00139   }
00140 
00141   const std::string &getName()
00142   {
00143     return imarker_msg.name;
00144   }
00145 
00146   void setName(const std::string &name)
00147   {
00148     imarker_msg.name = name;
00149   }
00150 
00151   bool isSelected(void)
00152   {
00153     return selected_;
00154   }
00155 
00156   bool isVisible()
00157   {
00158     return (imarker);
00159   }
00160 
00161   void connect(const QObject * receiver, const char * method)
00162   {
00163     receiver_ = receiver;
00164     receiver_method_ = method;
00165     QObject::connect( imarker.get(), SIGNAL( userFeedback(visualization_msgs::InteractiveMarkerFeedback &)), receiver, method );
00166   }
00167 
00168   virtual ~FrameMarker()
00169   {}
00170 
00171 protected:
00172   virtual void buildFrom(const std::string &name, const std::string &frame_id, const geometry_msgs::Pose &pose, double scale, const std_msgs::ColorRGBA &color);
00173   virtual void rebuild();
00174 
00175   std::vector<visualization_msgs::MenuEntry> menu_entries_;
00176 
00177   Ogre::SceneNode *parent_node_;
00178   rviz::DisplayContext* context_;
00179 
00180   bool selected_;
00181   bool visible_x_, visible_y_, visible_z_;
00182   std_msgs::ColorRGBA color_;
00183 
00184   Ogre::Vector3 position_;
00185   Ogre::Quaternion orientation_;
00186 
00187   const QObject *receiver_;
00188   const char *receiver_method_;
00189 };
00190 
00191 
00196 class GripperMarker : public FrameMarker
00197 {
00198 public:
00199   typedef enum
00200   {
00201     NOT_TESTED, PROCESSING, REACHABLE, NOT_REACHABLE, IN_COLLISION
00202   } GripperMarkerState;
00203 
00205   GripperMarker(const GripperMarker &gripper_marker) : FrameMarker(gripper_marker)
00206   {
00207     robot_state_ = gripper_marker.robot_state_;
00208     eef_ = gripper_marker.eef_;
00209     state_ = gripper_marker.state_;
00210     display_gripper_mesh_ = gripper_marker.display_gripper_mesh_;
00211 
00212     rebuild();
00213   }
00214 
00228   GripperMarker(const robot_state::RobotState& robot_state, Ogre::SceneNode *parent_node, rviz::DisplayContext *context, const std::string &name,
00229                 const std::string &frame_id, const robot_interaction::RobotInteraction::EndEffector &eef, const geometry_msgs::Pose &pose, double scale,
00230                 const GripperMarkerState &state, bool is_selected = false, bool visible_x = true, bool visible_y = true, bool visible_z = true);
00231 
00232   virtual void select(bool display_gripper_mesh = true);
00233   virtual void unselect(bool display_gripper_mesh = false);
00234 
00235   virtual void setState(const GripperMarkerState &state)
00236   {
00237     if (state != state_)
00238     {
00239       state_ = state;
00240       const float *color = stateToColor(state);
00241       setColor(color[0], color[1], color[2], color[3]);
00242     }
00243   }
00244 
00245   const GripperMarkerState &getState()
00246   {
00247     return state_;
00248   }
00249 
00250   void setRobotState(const robot_state::RobotState& robot_state)
00251   {
00252     robot_state_ = &robot_state;
00253   }
00254 
00255   const robot_state::RobotState *getRobotState()
00256   {
00257     return robot_state_;
00258   }
00259 
00260   void setEndEffector(const robot_interaction::RobotInteraction::EndEffector &eef)
00261   {
00262     eef_ = eef;
00263   }
00264 
00265   const robot_interaction::RobotInteraction::EndEffector getEndEffector()
00266   {
00267     return eef_;
00268   }
00269 
00270 protected:
00271   static const float GOAL_NOT_TESTED_COLOR[4];
00272   static const float GOAL_PROCESSING_COLOR[4];
00273   static const float GOAL_NOT_REACHABLE_COLOR[4];
00274   static const float GOAL_REACHABLE_COLOR[4];
00275   static const float GOAL_COLLISION_COLOR[4];
00276 
00277   virtual void buildFrom(const std::string &name, const std::string &frame_id, const geometry_msgs::Pose &pose, double scale, const std_msgs::ColorRGBA &color);
00278 
00279   const float *stateToColor(const GripperMarkerState &state);
00280 
00281   const robot_state::RobotState *robot_state_;
00282   robot_interaction::RobotInteraction::EndEffector eef_;
00283 
00284   bool display_gripper_mesh_;
00285   GripperMarkerState state_;
00286 };
00287 
00288 typedef boost::shared_ptr<GripperMarker> GripperMarkerPtr;
00289 
00290 } //namespace
00291 #endif


benchmarks_gui
Author(s): Mario Prats
autogenerated on Wed Aug 26 2015 12:44:27