43 server_(
ros::NodeHandle(),
45 boost::bind(&BufferServer::goalCB, this,
boost::placeholders::_1),
46 boost::bind(&BufferServer::cancelCB, this,
boost::placeholders::_1),
56 boost::mutex::scoped_lock l(
mutex_);
65 tf2_msgs::LookupTransformResult result;
74 result.error.error = result.error.CONNECTIVITY_ERROR;
75 result.error.error_string = ex.what();
79 result.error.error = result.error.LOOKUP_ERROR;
80 result.error.error_string = ex.what();
84 result.error.error = result.error.EXTRAPOLATION_ERROR;
85 result.error.error_string = ex.what();
89 result.error.error = result.error.INVALID_ARGUMENT_ERROR;
90 result.error.error_string = ex.what();
94 result.error.error = result.error.TIMEOUT_ERROR;
95 result.error.error_string = ex.what();
99 result.error.error = result.error.TRANSFORM_ERROR;
100 result.error.error_string = ex.what();
106 info.handle.setSucceeded(result);
116 boost::mutex::scoped_lock l(
mutex_);
122 GoalInfo& info = *it;
123 if(info.handle == gh)
125 info.handle.setCanceled();
142 goal_info.handle = gh;
149 tf2_msgs::LookupTransformResult result;
156 result.error.error = result.error.CONNECTIVITY_ERROR;
157 result.error.error_string = ex.what();
161 result.error.error = result.error.LOOKUP_ERROR;
162 result.error.error_string = ex.what();
166 result.error.error = result.error.EXTRAPOLATION_ERROR;
167 result.error.error_string = ex.what();
171 result.error.error = result.error.INVALID_ARGUMENT_ERROR;
172 result.error.error_string = ex.what();
176 result.error.error = result.error.TIMEOUT_ERROR;
177 result.error.error_string = ex.what();
181 result.error.error = result.error.TRANSFORM_ERROR;
182 result.error.error_string = ex.what();
185 gh.setSucceeded(result);
189 boost::mutex::scoped_lock l(
mutex_);
195 const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal();
202 goal->source_frame, goal->source_time, goal->fixed_frame);
207 const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal();
214 goal->source_frame, goal->source_time, goal->fixed_frame);