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 #include "pr2_gripper_click/gripper_click_display.h"
00031 #include "pr2_gripper_click/gripper_click_rviz_ui.h"
00032
00033 #include <rviz/window_manager_interface.h>
00034 #include <rviz/render_panel.h>
00035 #include <rviz/visualization_manager.h>
00036
00037 namespace pr2_gripper_click {
00038
00039 template<class ActionClass>
00040 GripperClickDisplay<ActionClass>::GripperClickDisplay( std::string action_topic, const std::string& name, rviz::VisualizationManager* manager ) :
00041 Display( name, manager ),
00042 gripper_click_rviz_ui_(0),
00043 action_server_(0),
00044 action_topic_(action_topic)
00045 {
00046 }
00047
00048 template<class ActionClass>
00049 GripperClickDisplay<ActionClass>::~GripperClickDisplay()
00050 {
00051 delete gripper_click_rviz_ui_;
00052 delete action_server_;
00053 }
00054
00055 template<class ActionClass>
00056 void GripperClickDisplay<ActionClass>::onEnable()
00057 {
00058 if ( !gripper_click_rviz_ui_ )
00059 {
00060 gripper_click_rviz_ui_ = new GripperClickRvizUI<ActionClass>(vis_manager_);
00061 }
00062 startActionServer( );
00063 }
00064
00065 template<class ActionClass>
00066 void GripperClickDisplay<ActionClass>::onDisable()
00067 {
00068 stopActionServer();
00069 }
00070
00071 template<class ActionClass>
00072 void GripperClickDisplay<ActionClass>::update(float wall_dt, float ros_dt)
00073 {
00074 if ( !action_server_->isActive() )
00075 {
00076 return;
00077 }
00078
00079 gripper_click_rviz_ui_->update(wall_dt, ros_dt);
00080
00081
00082 switch( gripper_click_rviz_ui_->getState() )
00083 {
00084 case GripperClickRvizUI<ActionClass>::IDLE:
00085 break;
00086 case GripperClickRvizUI<ActionClass>::CANCELED:
00087 abortAction();
00088 break;
00089 case GripperClickRvizUI<ActionClass>::ACCEPTED:
00090 {
00091 ResultType result;
00092 if ( !gripper_click_rviz_ui_->getResult( result ) )
00093 {
00094 ROS_ERROR( "Could not retrieve result!" );
00095 abortAction();
00096 }
00097 else
00098 {
00099 ROS_INFO( "Action was successful. Sending back result." );
00100 action_server_->setSucceeded( result );
00101 gripper_click_rviz_ui_->hide();
00102 }
00103 break;
00104 }
00105 }
00106 }
00107
00108 template<class ActionClass>
00109 void GripperClickDisplay<ActionClass>::startActionServer( )
00110 {
00111 if ( action_server_ )
00112 {
00113 ROS_ERROR( "Action server already started!" );
00114 return;
00115 }
00116
00117 ROS_INFO("Starting action server.");
00118
00119
00120 action_server_ =
00121 new actionlib::SimpleActionServer<ActionClass>( update_nh_, action_topic_, false );
00122
00123 action_server_->registerGoalCallback( boost::bind(&GripperClickDisplay::acceptNewGoal, this) );
00124 action_server_->registerPreemptCallback( boost::bind(&GripperClickDisplay::preemptAction, this) );
00125
00126 action_server_->start();
00127 }
00128
00129
00130 template<class ActionClass>
00131 void GripperClickDisplay<ActionClass>::stopActionServer()
00132 {
00133 if ( !action_server_ )
00134 {
00135 ROS_ERROR("Action server cannot be stopped because it is not running.");
00136 return;
00137 }
00138
00139
00140 if ( action_server_->isActive() )
00141 {
00142 ROS_WARN("Aborting action goal.");
00143 action_server_->setAborted();
00144
00145 }
00146
00147 ROS_INFO("Stopping action server.");
00148 delete action_server_;
00149 action_server_ = 0;
00150 }
00151
00152 template<class ActionClass>
00153 void GripperClickDisplay<ActionClass>::acceptNewGoal()
00154 {
00155 const typename GoalType::ConstPtr &goal = action_server_->acceptNewGoal();
00156
00157 gripper_click_rviz_ui_->setImage( goal->sensor_data.image, goal->sensor_data.disparity_image, goal->sensor_data.camera_info );
00158
00159 if ( !gripper_click_rviz_ui_->setGoal( goal ) )
00160 {
00161 ROS_ERROR( "Couldn't set new goal!" );
00162 action_server_->setAborted();
00163 gripper_click_rviz_ui_->hide();
00164 }
00165 }
00166
00167 template<class ActionClass>
00168 void GripperClickDisplay<ActionClass>::preemptAction()
00169 {
00170 action_server_->setAborted();
00171 gripper_click_rviz_ui_->hide();
00172 }
00173
00174 template<class ActionClass>
00175 void GripperClickDisplay<ActionClass>::abortAction()
00176 {
00177 ROS_WARN( "Action was canceled." );
00178 action_server_->setPreempted();
00179 gripper_click_rviz_ui_->hide();
00180 }
00181
00182
00183 template class GripperClickDisplay<GripperClickPickupAction>;
00184 template class GripperClickDisplay<GripperClickPlaceAction>;
00185
00186 }