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