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
00031
00032
00033
00034
00035
00036
00037 #ifndef ACTIONLIB_CLIENT_SERVICE_CLIENT_IMP_H_
00038 #define ACTIONLIB_CLIENT_SERVICE_CLIENT_IMP_H_
00039 namespace actionlib {
00040 template <class ActionSpec>
00041 ServiceClientImpT<ActionSpec>::ServiceClientImpT(ros::NodeHandle n, std::string name){
00042 ac_.reset(new SimpleActionClientT(n, name, true));
00043 }
00044
00045 template <class ActionSpec>
00046 bool ServiceClientImpT<ActionSpec>::waitForServer(const ros::Duration& timeout){
00047 return ac_->waitForServer(timeout);
00048 }
00049
00050 template <class ActionSpec>
00051 bool ServiceClientImpT<ActionSpec>::isServerConnected(){
00052 return ac_->isServerConnected();
00053 }
00054
00055 template <class ActionSpec>
00056 bool ServiceClientImpT<ActionSpec>::call(const void* goal, std::string goal_md5sum,
00057 void* result, std::string result_md5sum)
00058 {
00059
00060 const Goal* goal_c = static_cast<const Goal*>(goal);
00061 Result* result_c = static_cast<Result*>(result);
00062
00063
00064 namespace mt = ros::message_traits;
00065
00066 if(strcmp(mt::md5sum(*goal_c), goal_md5sum.c_str()) || strcmp(mt::md5sum(*result_c), result_md5sum.c_str()))
00067 {
00068 ROS_ERROR_NAMED("actionlib", "Incorrect md5Sums for goal and result types");
00069 return false;
00070 }
00071
00072 if(!ac_->isServerConnected()){
00073 ROS_ERROR_NAMED("actionlib", "Attempting to make a service call when the server isn't actually connected to the client.");
00074 return false;
00075 }
00076
00077 ac_->sendGoalAndWait(*goal_c);
00078 if(ac_->getState() == SimpleClientGoalState::SUCCEEDED){
00079 (*result_c) = *(ac_->getResult());
00080 return true;
00081 }
00082
00083 return false;
00084 }
00085
00086
00087 template <class Goal, class Result>
00088 bool ServiceClient::call(const Goal& goal, Result& result){
00089 namespace mt = ros::message_traits;
00090 return client_->call(&goal, mt::md5sum(goal), &result, mt::md5sum(result));
00091 }
00092
00093 bool ServiceClient::waitForServer(const ros::Duration& timeout){
00094 return client_->waitForServer(timeout);
00095 }
00096
00097 bool ServiceClient::isServerConnected(){
00098 return client_->isServerConnected();
00099 }
00100
00101
00102 template <class ActionSpec>
00103 ServiceClient serviceClient(ros::NodeHandle n, std::string name){
00104 boost::shared_ptr<ServiceClientImp> client_ptr(new ServiceClientImpT<ActionSpec>(n, name));
00105 return ServiceClient(client_ptr);
00106 }
00107
00108 };
00109 #endif