Fawkes API Fawkes Development Version
clips_tf_thread.cpp
1
2/***************************************************************************
3 * clips_navgraph_thread.cpp - NavGraph feature for CLIPS
4 *
5 * Created: Wed Oct 09 19:27:41 2013
6 * Copyright 2006-2013 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "clips_tf_thread.h"
23
24using namespace fawkes;
25
26/** @class ClipsTFThread "clips-protobuf-thread.h"
27 * Provide protobuf functionality to CLIPS environment.
28 * @author Tim Niemueller
29 */
30
31/** Constructor. */
33: Thread("ClipsTFThread", Thread::OPMODE_WAITFORWAKEUP),
34 CLIPSFeature("tf"),
36 debug_(true)
37{
38}
39
40/** Destructor. */
42{
43}
44
45void
47{
48 try {
49 debug_ = config->get_bool("/plugins/clips-tf/debug");
50 } catch (...) {
51 }
52}
53
54void
56{
57 envs_.clear();
58}
59
60void
62{
63 envs_[env_name] = clips;
64 logger->log_debug(name(), "Called to initialize environment %s", env_name.c_str());
65
66 clips.lock();
67 //clips->batch_evaluate(SRCDIR"/clips/navgraph.clp");
68
69 clips->add_function("tf-quat-from-yaw",
70 sigc::slot<CLIPS::Values, double>(
71 sigc::mem_fun(*this, &ClipsTFThread::clips_tf_quat_from_yaw)));
72 clips->add_function("tf-yaw-from-quat",
73 sigc::slot<double, CLIPS::Values>(
74 sigc::mem_fun(*this, &ClipsTFThread::clips_tf_yaw_from_quat)));
75
76 clips->add_function("tf-frame-exists",
77 sigc::slot<CLIPS::Value, std::string>(
78 sigc::mem_fun(*this, &ClipsTFThread::clips_tf_frame_exists)));
79 clips->add_function("tf-can-transform",
80 sigc::slot<CLIPS::Value, std::string, std::string, CLIPS::Values>(
81 sigc::mem_fun(*this, &ClipsTFThread::clips_tf_can_transform)));
82
83 clips->add_function(
84 "tf-transform-point",
85 sigc::slot<CLIPS::Values, std::string, std::string, CLIPS::Values, CLIPS::Values>(
86 sigc::mem_fun(*this, &ClipsTFThread::clips_tf_transform_point)));
87 clips->add_function(
88 "tf-transform-vector",
89 sigc::slot<CLIPS::Values, std::string, std::string, CLIPS::Values, CLIPS::Values>(
90 sigc::mem_fun(*this, &ClipsTFThread::clips_tf_transform_vector)));
91 clips->add_function(
92 "tf-transform-quaternion",
93 sigc::slot<CLIPS::Values, std::string, std::string, CLIPS::Values, CLIPS::Values>(
94 sigc::mem_fun(*this, &ClipsTFThread::clips_tf_transform_quaternion)));
95 clips->add_function(
96 "tf-transform-pose",
97 sigc::
98 slot<CLIPS::Values, std::string, std::string, CLIPS::Values, CLIPS::Values, CLIPS::Values>(
99 sigc::mem_fun(*this, &ClipsTFThread::clips_tf_transform_pose)));
100
101 clips.unlock();
102}
103
104void
105ClipsTFThread::clips_context_destroyed(const std::string &env_name)
106{
107 envs_.erase(env_name);
108 logger->log_debug(name(), "Removing environment %s", env_name.c_str());
109}
110
111CLIPS::Value
112ClipsTFThread::clips_tf_frame_exists(std::string frame_id)
113{
114 return CLIPS::Value(tf_listener->frame_exists(frame_id) ? "TRUE" : "FALSE", CLIPS::TYPE_SYMBOL);
115}
116
117CLIPS::Value
118ClipsTFThread::clips_tf_can_transform(std::string target_frame,
119 std::string source_frame,
120 CLIPS::Values time)
121{
122 if (!validate_time(time)) {
123 return CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL);
124 }
125
126 fawkes::Time t(convert_time(time));
127 return CLIPS::Value(tf_listener->can_transform(target_frame, source_frame, t) ? "TRUE" : "FALSE",
128 CLIPS::TYPE_SYMBOL);
129}
130
131CLIPS::Values
132ClipsTFThread::clips_tf_transform_point(std::string target_frame,
133 std::string source_frame,
134 CLIPS::Values time,
135 CLIPS::Values point)
136{
137 if (!(validate_time(time) && validate_point(point))) {
138 return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
139 }
140
141 fawkes::Time t(convert_time(time));
143 tf::Point(point[0].as_float(), point[1].as_float(), point[2].as_float()), t, source_frame);
145
146 try {
147 tf_listener->transform_point(target_frame, in, out);
148
149 if (debug_)
151 "Transformed point %s->%s: (%.2f,%.2f,%.2f) -> (%.2f,%.2f,%.2f)",
152 source_frame.c_str(),
153 target_frame.c_str(),
154 in.x(),
155 in.y(),
156 in.z(),
157 out.x(),
158 out.y(),
159 out.z());
160
161 CLIPS::Values rv(3, CLIPS::Value(0.));
162 rv[0] = out.x();
163 rv[1] = out.y();
164 rv[2] = out.z();
165 return rv;
166 } catch (Exception &e) {
168 "Failed to transform point from %s to %s: %s",
169 source_frame.c_str(),
170 target_frame.c_str(),
172 return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
173 }
174}
175
176CLIPS::Values
177ClipsTFThread::clips_tf_transform_vector(std::string target_frame,
178 std::string source_frame,
179 CLIPS::Values time,
180 CLIPS::Values vector3)
181{
182 if (!(validate_time(time) && validate_vector3(vector3))) {
183 return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
184 }
185
186 fawkes::Time t(convert_time(time));
187 tf::Stamped<tf::Vector3> in(tf::Vector3(vector3[0].as_float(),
188 vector3[1].as_float(),
189 vector3[2].as_float()),
190 t,
191 source_frame);
193
194 try {
195 tf_listener->transform_vector(target_frame, in, out);
196
197 if (debug_)
199 "Transformed vector %s->%s: (%.2f,%.2f,%.2f) -> (%.2f,%.2f,%.2f)",
200 source_frame.c_str(),
201 target_frame.c_str(),
202 in.x(),
203 in.y(),
204 in.z(),
205 out.x(),
206 out.y(),
207 out.z());
208
209 CLIPS::Values rv(3, CLIPS::Value(0.));
210 rv[0] = out.x();
211 rv[1] = out.y();
212 rv[2] = out.z();
213 return rv;
214 } catch (Exception &e) {
216 "Failed to transform vector from %s to %s: %s",
217 source_frame.c_str(),
218 target_frame.c_str(),
220 return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
221 }
222}
223
224CLIPS::Values
225ClipsTFThread::clips_tf_transform_quaternion(std::string target_frame,
226 std::string source_frame,
227 CLIPS::Values time,
228 CLIPS::Values quat)
229{
230 if (!(validate_time(time) && validate_quat(quat))) {
231 return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
232 }
233
234 fawkes::Time t(convert_time(time));
236 tf::Quaternion(quat[0].as_float(), quat[1].as_float(), quat[2].as_float(), quat[3].as_float()),
237 t,
238 source_frame);
240
241 try {
242 tf_listener->transform_quaternion(target_frame, in, out);
243
244 if (debug_)
246 "Transformed quaternion %s->%s: "
247 "(%.2f,%.2f,%.2f,%.2f) -> (%.2f,%.2f,%.2f,%.2f)",
248 source_frame.c_str(),
249 target_frame.c_str(),
250 in.x(),
251 in.y(),
252 in.z(),
253 in.w(),
254 out.x(),
255 out.y(),
256 out.z(),
257 out.w());
258
259 CLIPS::Values rv(4, CLIPS::Value(0.));
260 rv[0] = out.x();
261 rv[1] = out.y();
262 rv[2] = out.z();
263 rv[3] = out.w();
264 return rv;
265 } catch (Exception &e) {
267 "Failed to transform vector quaternion %s to %s: %s",
268 source_frame.c_str(),
269 target_frame.c_str(),
271 return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
272 }
273}
274
275CLIPS::Values
276ClipsTFThread::clips_tf_transform_pose(std::string target_frame,
277 std::string source_frame,
278 CLIPS::Values time,
279 CLIPS::Values translation,
280 CLIPS::Values rotation_quat)
281{
282 if (!(validate_time(time) && validate_vector3(translation) && validate_quat(rotation_quat))) {
283 return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
284 }
285
286 fawkes::Time t(convert_time(time));
287 tf::Stamped<tf::Pose> in(tf::Pose(tf::Quaternion(rotation_quat[0].as_float(),
288 rotation_quat[1].as_float(),
289 rotation_quat[2].as_float(),
290 rotation_quat[3].as_float()),
291 tf::Vector3(translation[0].as_float(),
292 translation[1].as_float(),
293 translation[2].as_float())),
294 t,
295 source_frame);
297
298 try {
299 tf_listener->transform_pose(target_frame, in, out);
300
301 tf::Quaternion in_q = in.getRotation();
302 tf::Quaternion out_q = out.getRotation();
303 if (debug_)
305 "Transformed pose %s->%s: "
306 "T(%.2f,%.2f,%.2f) R(%.2f,%.2f,%.2f,%.2f) -> "
307 "T(%.2f,%.2f,%.2f) R(%.2f,%.2f,%.2f,%.2f)",
308 source_frame.c_str(),
309 target_frame.c_str(),
310 in.getOrigin().x(),
311 in.getOrigin().y(),
312 in.getOrigin().z(),
313 in_q.x(),
314 in_q.y(),
315 in_q.z(),
316 in_q.w(),
317 out.getOrigin().x(),
318 out.getOrigin().y(),
319 out.getOrigin().z(),
320 out_q.x(),
321 out_q.y(),
322 out_q.z(),
323 out_q.w());
324
325 CLIPS::Values rv(7, CLIPS::Value(0.));
326 rv[0] = out.getOrigin().x();
327 rv[1] = out.getOrigin().y();
328 rv[2] = out.getOrigin().z();
329 rv[3] = out_q.x();
330 rv[4] = out_q.y();
331 rv[5] = out_q.z();
332 rv[6] = out_q.w();
333 return rv;
334 } catch (Exception &e) {
336 "Failed to transform pose from %s to %s: %s",
337 source_frame.c_str(),
338 target_frame.c_str(),
340 return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
341 }
342}
343
344CLIPS::Values
345ClipsTFThread::clips_tf_quat_from_yaw(double yaw)
346{
347 tf::Quaternion q = tf::create_quaternion_from_yaw(yaw);
348 CLIPS::Values rv(4, CLIPS::Value(0.));
349 rv[0] = q.x();
350 rv[1] = q.y();
351 rv[2] = q.z();
352 rv[3] = q.w();
353 return rv;
354}
355
356double
357ClipsTFThread::clips_tf_yaw_from_quat(CLIPS::Values quat)
358{
359 tf::Quaternion q(quat[0].as_float(), quat[1].as_float(), quat[2].as_float(), quat[3].as_float());
360 return tf::get_yaw(q);
361}
362
363void
365{
366}
367
368bool
369ClipsTFThread::validate_time(const CLIPS::Values &time)
370{
371 if (time.size() != 2) {
372 logger->log_warn(name(), "Invalid time: must be list of exactly two entries");
373 return false;
374 }
375 for (auto &t : time) {
376 CLIPS::Type t_type = t.type();
377 if (t_type != CLIPS::TYPE_INTEGER) {
378 logger->log_warn(name(), "Invalid time: must be list of integers");
379 return false;
380 }
381 }
382 return true;
383}
384
386ClipsTFThread::convert_time(const CLIPS::Values &time)
387{
388 if (!validate_time(time))
389 return fawkes::Time(0, 0);
390
391 return fawkes::Time(time[0].as_integer(), time[1].as_integer());
392}
393
394bool
395ClipsTFThread::validate_point(const CLIPS::Values &point)
396{
397 if (point.size() != 3) {
398 logger->log_warn(name(), "Invalid point: must be list of exactly three entries");
399 return false;
400 }
401 for (auto &c : point) {
402 CLIPS::Type c_type = c.type();
403 if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
404 logger->log_warn(name(), "Invalid point: must be list of floats or integers");
405 return false;
406 }
407 }
408 return true;
409}
410
411bool
412ClipsTFThread::validate_vector3(const CLIPS::Values &vector3)
413{
414 if (vector3.size() != 3) {
415 logger->log_warn(name(), "Invalid vector: must be list of exactly three entries");
416 return false;
417 }
418 for (auto &c : vector3) {
419 CLIPS::Type c_type = c.type();
420 if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
421 logger->log_warn(name(), "Invalid vector: must be list of floats or integers");
422 return false;
423 }
424 }
425 return true;
426}
427
428bool
429ClipsTFThread::validate_quat(const CLIPS::Values &quat)
430{
431 if (quat.size() != 4) {
432 logger->log_warn(name(), "Invalid quaternion: must be list of exactly four entries");
433 return false;
434 }
435 for (auto &c : quat) {
436 CLIPS::Type c_type = c.type();
437 if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
438 logger->log_warn(name(), "Invalid quaternion: must be list of floats or integers");
439 return false;
440 }
441 }
442 return true;
443}
virtual void clips_context_init(const std::string &env_name, fawkes::LockPtr< CLIPS::Environment > &clips)
Initialize a CLIPS context to use the provided feature.
virtual ~ClipsTFThread()
Destructor.
ClipsTFThread()
Constructor.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
virtual void clips_context_destroyed(const std::string &env_name)
Notification that a CLIPS environment has been destroyed.
Thread aspect to provide a feature to CLIPS environments.
Definition: clips_feature.h:58
CLIPS feature maintainer.
Definition: clips_feature.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:257
void unlock() const
Unlock object mutex.
Definition: lockptr.h:273
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
void transform_quaternion(const std::string &target_frame, const Stamped< Quaternion > &stamped_in, Stamped< Quaternion > &stamped_out) const
Transform a stamped Quaternion into the target frame.
void transform_point(const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const
Transform a stamped point into the target frame.
void transform_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
bool can_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
void transform_vector(const std::string &target_frame, const Stamped< Vector3 > &stamped_in, Stamped< Vector3 > &stamped_out) const
Transform a stamped vector into the target frame.
Fawkes library namespace.