Go to the documentation of this file.00001 #include "wam_arm_navigation_alg_node.h"
00002
00003 WamArmNavigationAlgNode::WamArmNavigationAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<WamArmNavigationAlgorithm>(),
00005 aborted(false),
00006 simple_pose_move_aserver_(public_node_handle_, "simple_pose_move"),
00007 move_iri_wam_client_("move_iri_wam", true),
00008 state_msg(""),
00009 state__("")
00010 {
00011
00012
00013
00014
00015
00016
00017 this->simple_pose_topic_subscriber_ = this->public_node_handle_.subscribe("simple_pose_topic", 100, &WamArmNavigationAlgNode::simple_pose_topic_callback, this);
00018
00019
00020
00021
00022
00023
00024 simple_pose_move_aserver_.registerStartCallback(boost::bind(&WamArmNavigationAlgNode::simple_pose_moveStartCallback, this, _1));
00025 simple_pose_move_aserver_.registerStopCallback(boost::bind(&WamArmNavigationAlgNode::simple_pose_moveStopCallback, this));
00026 simple_pose_move_aserver_.registerIsFinishedCallback(boost::bind(&WamArmNavigationAlgNode::simple_pose_moveIsFinishedCallback, this));
00027 simple_pose_move_aserver_.registerHasSucceedCallback(boost::bind(&WamArmNavigationAlgNode::simple_pose_moveHasSucceedCallback, this));
00028 simple_pose_move_aserver_.registerGetResultCallback(boost::bind(&WamArmNavigationAlgNode::simple_pose_moveGetResultCallback, this, _1));
00029 simple_pose_move_aserver_.registerGetFeedbackCallback(boost::bind(&WamArmNavigationAlgNode::simple_pose_moveGetFeedbackCallback, this, _1));
00030 simple_pose_move_aserver_.start();
00031
00032
00033 }
00034
00035 WamArmNavigationAlgNode::~WamArmNavigationAlgNode(void)
00036 {
00037
00038 }
00039
00040 void WamArmNavigationAlgNode::mainNodeThread(void)
00041 {
00042
00043
00044
00045
00046
00047
00048
00049
00050 }
00051
00052
00053 void WamArmNavigationAlgNode::simple_pose_topic_callback(const iri_wam_arm_navigation::PoseSimple::ConstPtr& msg)
00054 {
00055 ROS_INFO("WamArmNavigationAlgNode::simple_pose_topic_callback: New Message Received");
00056
00057
00058
00059 this->alg_.lock();
00060
00061 this->alg_.setTarget(msg->goal,msg->header.frame_id);
00062 arm_navigation_msgs::MoveArmGoal move_arm;
00063 makeMoveMsg(move_arm);
00064 move_iri_wamMakeActionRequest(move_arm);
00065
00066
00067
00068 this->alg_.unlock();
00069
00070 }
00071
00072
00073
00074
00075 void WamArmNavigationAlgNode::move_iri_wamDone(const actionlib::SimpleClientGoalState& state, const arm_navigation_msgs::MoveArmResultConstPtr& result)
00076 {
00077
00078 ROS_INFO("WamArmNavigationAlgNode::move_iri_wamDone: %s", state.toString().c_str());
00079 if( state.state_ == actionlib::SimpleClientGoalState::SUCCEEDED )
00080 {
00081 aborted=false;
00082 state__="FREE";
00083 }
00084 else
00085 {
00086 aborted=true;
00087 state__="ABORTED";
00088 }
00089 code_result=result->error_code;
00090
00091 }
00092
00093 void WamArmNavigationAlgNode::move_iri_wamActive()
00094 {
00095
00096 }
00097
00098 void WamArmNavigationAlgNode::move_iri_wamFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback)
00099 {
00100
00101
00102 bool feedback_is_ok = true;
00103
00104
00105
00106 state_msg= feedback->state;
00107
00108 if( !feedback_is_ok )
00109 {
00110 move_iri_wam_client_.cancelGoal();
00111
00112 }
00113 }
00114
00115 void WamArmNavigationAlgNode::simple_pose_moveStartCallback(const iri_wam_arm_navigation::SimplePoseGoalConstPtr& goal)
00116 {
00117 alg_.lock();
00118
00119 alg_.setTarget(goal->goal,goal->frame_id);
00120 arm_navigation_msgs::MoveArmGoal move_arm;
00121 makeMoveMsg(move_arm);
00122 move_iri_wamMakeActionRequest(move_arm);
00123
00124 state__="NO FREE";
00125 aborted=false;
00126
00127 ROS_INFO_STREAM(goal->goal);
00128
00129 alg_.unlock();
00130 }
00131
00132 void WamArmNavigationAlgNode::simple_pose_moveStopCallback(void)
00133 {
00134 alg_.lock();
00135
00136 arm_navigation_msgs::MoveArmGoal move_arm;
00137 makeMoveMsg(move_arm);
00138 move_iri_wamMakeActionRequest(move_arm);
00139 alg_.unlock();
00140 }
00141
00142 bool WamArmNavigationAlgNode::simple_pose_moveIsFinishedCallback(void)
00143 {
00144 bool ret = false;
00145 alg_.lock();
00146
00147 if( (state_msg.compare("SUCCEEDED") == 0 ) ||
00148 (state_msg.compare("LOST") == 0 ) ||
00149 (state_msg.compare("ABORTED") == 0 ) ||
00150 (state_msg.compare("FAILED") == 0 ) )
00151 {
00152 ret = true;
00153 }
00154 alg_.unlock();
00155
00156 return ret;
00157 }
00158
00159 bool WamArmNavigationAlgNode::simple_pose_moveHasSucceedCallback(void)
00160 {
00161 bool ret = false;
00162
00163 alg_.lock();
00164
00165 if(code_result.val==arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS){
00166 ret=true;
00167 }
00168
00169
00170 alg_.unlock();
00171
00172 return ret;
00173 }
00174
00175 void WamArmNavigationAlgNode::simple_pose_moveGetResultCallback(iri_wam_arm_navigation::SimplePoseResultPtr& result)
00176 {
00177 alg_.lock();
00178
00179 (*result).error_code=code_result;
00180
00181 alg_.unlock();
00182 }
00183
00184 void WamArmNavigationAlgNode::simple_pose_moveGetFeedbackCallback(iri_wam_arm_navigation::SimplePoseFeedbackPtr& feedback)
00185 {
00186 alg_.lock();
00187
00188
00189
00190
00191 if (state__.compare("FREE")==0){
00192 (*feedback).state="FREE";
00193 }
00194 else if (state__.compare("ABORTED")==0){
00195 (*feedback).state="ABORTED";
00196 }
00197 else (*feedback).state="NO FREE";
00198
00199 (*feedback).succesed=(state_msg.compare("SUCCEEDED"))?true:false;
00200 alg_.unlock();
00201 }
00202
00203
00204 void WamArmNavigationAlgNode::move_iri_wamMakeActionRequest(const arm_navigation_msgs::MoveArmGoal& msg)
00205 {
00206 ROS_INFO("WamArmNavigationAlgNode::move_iri_wamMakeActionRequest: Starting New Request!");
00207
00208
00209
00210 move_iri_wam_client_.waitForServer();
00211 ROS_INFO("WamArmNavigationAlgNode::move_iri_wamMakeActionRequest: Server is Available!");
00212
00213
00214
00215 move_iri_wam_client_.sendGoal(msg,
00216 boost::bind(&WamArmNavigationAlgNode::move_iri_wamDone, this, _1, _2),
00217 boost::bind(&WamArmNavigationAlgNode::move_iri_wamActive, this),
00218 boost::bind(&WamArmNavigationAlgNode::move_iri_wamFeedback, this, _1));
00219 this->alg_.sendedPose();
00220 ROS_INFO("WamArmNavigationAlgNode::move_iri_wamMakeActionRequest: Goal Sent. Wait for Result!");
00221
00222
00223 }
00224
00225 void WamArmNavigationAlgNode::node_config_update(Config &config, uint32_t level)
00226 {
00227 this->alg_.lock();
00228
00229 this->alg_.unlock();
00230 }
00231
00232 void WamArmNavigationAlgNode::addNodeDiagnostics(void)
00233 {
00234 }
00235 void WamArmNavigationAlgNode::makeMoveMsg(arm_navigation_msgs::MoveArmGoal& goal)
00236 {
00237 if(this->alg_.hasPose())
00238 {
00239 this->alg_.setPlannerRequest(goal);
00240 this->alg_.setPlannerRequestPose(goal);
00241 }
00242 else
00243 {
00244 arm_navigation_msgs::MoveArmGoal empty;
00245 goal=empty;
00246 }
00247 }
00248
00249
00250 int main(int argc,char *argv[])
00251 {
00252 return algorithm_base::main<WamArmNavigationAlgNode>(argc, argv, "wam_arm_navigation_alg_node");
00253
00254 }