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


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