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