Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef GRIPPER_CLICK_GRASP_ADJUST
00031 #define GRIPPER_CLICK_GRASP_ADJUST
00032
00033 #include "pr2_gripper_click/gripper_click_rviz_ui.h"
00034 #include "rviz_interaction_tools/gripper.h"
00035
00036 #include <tf/transform_datatypes.h>
00037
00038 #include <stereo_msgs/DisparityImage.h>
00039 #include <sensor_msgs/CameraInfo.h>
00040
00041 #include <actionlib/client/simple_action_client.h>
00042 #include <pr2_grasp_adjust/GraspAdjustAction.h>
00043
00044 typedef actionlib::SimpleActionClient<pr2_grasp_adjust::GraspAdjustAction> ActionClient;
00045
00046
00047
00048 namespace Ogre {
00049 class SceneNode;
00050 }
00051
00052 namespace rviz_interaction_tools {
00053 class DisparityRenderer;
00054 }
00055
00056 namespace pr2_gripper_click {
00057
00058 class GripperClickGraspAdjust : public GripperClickPlugin<GripperClickPickupAction>
00059 {
00060
00061 public:
00062
00063 GripperClickGraspAdjust();
00064
00065 ~GripperClickGraspAdjust();
00066
00067
00068 virtual void init( rviz::VisualizationManager *vis_manager, Ogre::SceneManager* scene_manager, Ogre::SceneNode* camera_node );
00069
00070 virtual bool setGoal( const GripperClickPickupGoal::ConstPtr &goal );
00071
00072 virtual bool getResult(GripperClickPickupResult &result);
00073
00074 virtual void update(float wall_dt, float ros_dt);
00075
00076
00077 virtual bool hasValidResult() { return valid_transform_; }
00078
00079
00080 virtual void show( );
00081 virtual void hide( );
00082
00083 virtual void getGripperParams( bool &visible, Ogre::Vector3 &position, Ogre::Quaternion &orientation, float &angle );
00084
00085 virtual void onRenderWindowMouseEvents( wxMouseEvent& event, Ogre::Ray &mouse_ray );
00086
00087 protected:
00088
00089 struct ClickInfo
00090 {
00091 ClickInfo() : down_(false), x_(0), y_(0), ref_(0.0) {}
00092 bool down_;
00093 int x_, y_;
00094 float ref_;
00095 };
00096
00097
00098 void requestGraspAdjustAction();
00099 void receiveGraspAdjustAction(const actionlib::SimpleClientGoalState &state,
00100 const pr2_grasp_adjust::GraspAdjustResultConstPtr &result);
00101
00102 int gripper_index_;
00103 std::vector<float> gripper_angle_;
00104 std::vector<float> gripper_score_;
00105 std::vector<float> gripper_roll_;
00106 std::vector<float> gripper_approach_;
00107 std::vector<btTransform> gripper_transform_;
00108
00109 ClickInfo rb_, mb_, lb_, dlb_;
00110
00111 bool valid_transform_;
00112
00113
00114 Ogre::ManualObject* line_object_;
00115
00116 stereo_msgs::DisparityImage disparity_image_;
00117 sensor_msgs::CameraInfo camera_info_;
00118
00119 Ogre::SceneNode *scene_node_;
00120
00121 ActionClient action_client_;
00122 };
00123
00124 }
00125
00126 #endif