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 
24 using 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"),
35  CLIPSFeatureAspect(this),
36  debug_(true)
37 {
38 }
39 
40 /** Destructor. */
42 {
43 }
44 
45 void
47 {
48  try {
49  debug_ = config->get_bool("/plugins/clips-tf/debug");
50  } catch (...) {
51  }
52 }
53 
54 void
56 {
57  envs_.clear();
58 }
59 
60 void
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 
104 void
105 ClipsTFThread::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 
111 CLIPS::Value
112 ClipsTFThread::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 
117 CLIPS::Value
118 ClipsTFThread::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 
131 CLIPS::Values
132 ClipsTFThread::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_)
150  logger->log_debug(name(),
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) {
167  logger->log_warn(name(),
168  "Failed to transform point from %s to %s: %s",
169  source_frame.c_str(),
170  target_frame.c_str(),
171  e.what_no_backtrace());
172  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
173  }
174 }
175 
176 CLIPS::Values
177 ClipsTFThread::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_)
198  logger->log_debug(name(),
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) {
215  logger->log_warn(name(),
216  "Failed to transform vector from %s to %s: %s",
217  source_frame.c_str(),
218  target_frame.c_str(),
219  e.what_no_backtrace());
220  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
221  }
222 }
223 
224 CLIPS::Values
225 ClipsTFThread::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_)
245  logger->log_debug(name(),
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) {
266  logger->log_warn(name(),
267  "Failed to transform vector quaternion %s to %s: %s",
268  source_frame.c_str(),
269  target_frame.c_str(),
270  e.what_no_backtrace());
271  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
272  }
273 }
274 
275 CLIPS::Values
276 ClipsTFThread::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_)
304  logger->log_debug(name(),
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) {
335  logger->log_warn(name(),
336  "Failed to transform pose from %s to %s: %s",
337  source_frame.c_str(),
338  target_frame.c_str(),
339  e.what_no_backtrace());
340  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
341  }
342 }
343 
344 CLIPS::Values
345 ClipsTFThread::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 
356 double
357 ClipsTFThread::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 
363 void
365 {
366 }
367 
368 bool
369 ClipsTFThread::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 
386 ClipsTFThread::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 
394 bool
395 ClipsTFThread::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 
411 bool
412 ClipsTFThread::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 
428 bool
429 ClipsTFThread::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 }
fawkes::tf::Transformer::frame_exists
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
Definition: transformer.cpp:176
fawkes::LockPtr< CLIPS::Environment >
ClipsTFThread::clips_context_destroyed
virtual void clips_context_destroyed(const std::string &env_name)
Definition: clips_tf_thread.cpp:105
fawkes::tf::Transformer::transform_pose
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.
Definition: transformer.cpp:446
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
ClipsTFThread::clips_context_init
virtual void clips_context_init(const std::string &env_name, fawkes::LockPtr< CLIPS::Environment > &clips)
Definition: clips_tf_thread.cpp:61
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
fawkes::tf::Stamped
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:139
fawkes::tf::Transformer::transform_quaternion
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.
Definition: transformer.cpp:361
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::LockPtr::unlock
void unlock() const
Unlock object mutex.
Definition: lockptr.h:289
fawkes
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::tf::Transformer::can_transform
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.
Definition: transformer.cpp:220
ClipsTFThread::ClipsTFThread
ClipsTFThread()
Constructor.
Definition: clips_tf_thread.cpp:32
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
fawkes::tf::Transformer::transform_vector
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.
Definition: transformer.cpp:389
fawkes::CLIPSFeature
Definition: clips_feature.h:41
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
fawkes::Time
Definition: time.h:98
fawkes::TransformAspect::tf_listener
tf::Transformer * tf_listener
Definition: tf.h:72
ClipsTFThread::loop
virtual void loop()
Code to execute in the thread.
Definition: clips_tf_thread.cpp:364
fawkes::Thread
Definition: thread.h:45
fawkes::LockPtr::lock
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:273
fawkes::tf::Transformer::transform_point
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.
Definition: transformer.cpp:420
fawkes::CLIPSFeatureAspect
Definition: clips_feature.h:57
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
ClipsTFThread::~ClipsTFThread
virtual ~ClipsTFThread()
Destructor.
Definition: clips_tf_thread.cpp:41
ClipsTFThread::finalize
virtual void finalize()
Finalize the thread.
Definition: clips_tf_thread.cpp:55
ClipsTFThread::init
virtual void init()
Initialize the thread.
Definition: clips_tf_thread.cpp:46
fawkes::Exception
Definition: exception.h:41