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"), CLIPSFeatureAspect(this)
35 {
36 }
37 
38 
39 /** Destructor. */
41 {
42 }
43 
44 
45 void
47 {
48 }
49 
50 
51 void
53 {
54  envs_.clear();
55 }
56 
57 
58 void
59 ClipsTFThread::clips_context_init(const std::string &env_name,
61 {
62  envs_[env_name] = clips;
63  logger->log_debug(name(), "Called to initialize environment %s", env_name.c_str());
64 
65  clips.lock();
66  //clips->batch_evaluate(SRCDIR"/clips/navgraph.clp");
67 
68  clips->add_function("tf-quat-from-yaw",
69  sigc::slot<CLIPS::Values, double>
70  (sigc::mem_fun(*this, &ClipsTFThread::clips_tf_quat_from_yaw)));
71  clips->add_function("tf-yaw-from-quat",
72  sigc::slot<double, CLIPS::Values>
73  (sigc::mem_fun(*this, &ClipsTFThread::clips_tf_yaw_from_quat)));
74 
75  clips->add_function("tf-frame-exists", sigc::slot<CLIPS::Value, std::string>
76  (sigc::mem_fun(*this, &ClipsTFThread::clips_tf_frame_exists)));
77  clips->add_function("tf-can-transform",
78  sigc::slot<CLIPS::Value, std::string, std::string, CLIPS::Values>
79  (sigc::mem_fun(*this, &ClipsTFThread::clips_tf_can_transform)));
80 
81  clips->add_function("tf-transform-point",
82  sigc::slot<CLIPS::Values, std::string, std::string, CLIPS::Values, CLIPS::Values>
83  (sigc::mem_fun(*this, &ClipsTFThread::clips_tf_transform_point)));
84  clips->add_function("tf-transform-vector",
85  sigc::slot<CLIPS::Values, std::string, std::string, CLIPS::Values, CLIPS::Values>
86  (sigc::mem_fun(*this, &ClipsTFThread::clips_tf_transform_vector)));
87  clips->add_function("tf-transform-quaternion",
88  sigc::slot<CLIPS::Values, std::string, std::string, CLIPS::Values, CLIPS::Values>
89  (sigc::mem_fun(*this, &ClipsTFThread::clips_tf_transform_quaternion)));
90  clips->add_function("tf-transform-pose",
91  sigc::slot<CLIPS::Values, std::string, std::string,
92  CLIPS::Values, CLIPS::Values, CLIPS::Values>
93  (sigc::mem_fun(*this, &ClipsTFThread::clips_tf_transform_pose)));
94 
95  clips.unlock();
96 }
97 
98 void
99 ClipsTFThread::clips_context_destroyed(const std::string &env_name)
100 {
101  envs_.erase(env_name);
102  logger->log_debug(name(), "Removing environment %s", env_name.c_str());
103 }
104 
105 CLIPS::Value
106 ClipsTFThread::clips_tf_frame_exists(std::string frame_id)
107 {
108  return CLIPS::Value(tf_listener->frame_exists(frame_id) ? "TRUE" : "FALSE",
109  CLIPS::TYPE_SYMBOL);
110 }
111 
112 
113 CLIPS::Value
114 ClipsTFThread::clips_tf_can_transform(std::string target_frame, std::string source_frame,
115  CLIPS::Values time)
116 {
117  if (! validate_time(time)) {
118  return CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL);
119  }
120 
121  fawkes::Time t(convert_time(time));
122  return CLIPS::Value(tf_listener->can_transform(target_frame, source_frame, t)
123  ? "TRUE" : "FALSE", CLIPS::TYPE_SYMBOL);
124 }
125 
126 
127 CLIPS::Values
128 ClipsTFThread::clips_tf_transform_point(std::string target_frame, std::string source_frame,
129  CLIPS::Values time, CLIPS::Values point)
130 {
131  if (! (validate_time(time) && validate_point(point))) {
132  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
133  }
134 
135  fawkes::Time t(convert_time(time));
137  in(tf::Point(point[0].as_float(), point[1].as_float(), point[2].as_float()),
138  t, source_frame);
140 
141  try {
142  tf_listener->transform_point(target_frame, in, out);
143 
144  logger->log_debug(name(), "Transformed point %s->%s: (%.2f,%.2f,%.2f) -> (%.2f,%.2f,%.2f)",
145  source_frame.c_str(), target_frame.c_str(),
146  in.x(), in.y(), in.z(), out.x(), out.y(), out.z());
147 
148  CLIPS::Values rv(3, CLIPS::Value(0.));
149  rv[0] = out.x();
150  rv[1] = out.y();
151  rv[2] = out.z();
152  return rv;
153  } catch (Exception &e) {
154  logger->log_warn(name(), "Failed to transform point from %s to %s: %s",
155  source_frame.c_str(), target_frame.c_str(), e.what_no_backtrace());
156  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
157  }
158 }
159 
160 CLIPS::Values
161 ClipsTFThread::clips_tf_transform_vector( std::string target_frame, std::string source_frame,
162  CLIPS::Values time, CLIPS::Values vector3)
163 {
164  if (! (validate_time(time) && validate_vector3(vector3))) {
165  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
166  }
167 
168  fawkes::Time t(convert_time(time));
170  in(tf::Vector3(vector3[0].as_float(), vector3[1].as_float(), vector3[2].as_float()),
171  t, source_frame);
173 
174  try {
175  tf_listener->transform_vector(target_frame, in, out);
176 
177  logger->log_debug(name(), "Transformed vector %s->%s: (%.2f,%.2f,%.2f) -> (%.2f,%.2f,%.2f)",
178  source_frame.c_str(), target_frame.c_str(),
179  in.x(), in.y(), in.z(), out.x(), out.y(), out.z());
180 
181  CLIPS::Values rv(3, CLIPS::Value(0.));
182  rv[0] = out.x();
183  rv[1] = out.y();
184  rv[2] = out.z();
185  return rv;
186  } catch (Exception &e) {
187  logger->log_warn(name(), "Failed to transform vector from %s to %s: %s",
188  source_frame.c_str(), target_frame.c_str(), e.what_no_backtrace());
189  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
190  }
191 }
192 
193 CLIPS::Values
194 ClipsTFThread::clips_tf_transform_quaternion(std::string target_frame, std::string source_frame,
195  CLIPS::Values time, CLIPS::Values quat)
196 {
197  if (! (validate_time(time) && validate_quat(quat))) {
198  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
199  }
200 
201  fawkes::Time t(convert_time(time));
203  in(tf::Quaternion(quat[0].as_float(), quat[1].as_float(),
204  quat[2].as_float(), quat[3].as_float()),
205  t, source_frame);
207 
208  try {
209  tf_listener->transform_quaternion(target_frame, in, out);
210 
211  logger->log_debug(name(), "Transformed quaternion %s->%s: "
212  "(%.2f,%.2f,%.2f,%.2f) -> (%.2f,%.2f,%.2f,%.2f)",
213  source_frame.c_str(), target_frame.c_str(),
214  in.x(), in.y(), in.z(), in.w(), out.x(), out.y(), out.z(), out.w());
215 
216  CLIPS::Values rv(4, CLIPS::Value(0.));
217  rv[0] = out.x();
218  rv[1] = out.y();
219  rv[2] = out.z();
220  rv[3] = out.w();
221  return rv;
222  } catch (Exception &e) {
223  logger->log_warn(name(), "Failed to transform vector quaternion %s to %s: %s",
224  source_frame.c_str(), target_frame.c_str(), e.what_no_backtrace());
225  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
226  }
227 }
228 
229 
230 CLIPS::Values
231 ClipsTFThread::clips_tf_transform_pose(std::string target_frame, std::string source_frame,
232  CLIPS::Values time,
233  CLIPS::Values translation, CLIPS::Values rotation_quat)
234 {
235  if (! (validate_time(time) && validate_vector3(translation) && validate_quat(rotation_quat)))
236  {
237  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
238  }
239 
240  fawkes::Time t(convert_time(time));
242  in(tf::Pose(tf::Quaternion(rotation_quat[0].as_float(), rotation_quat[1].as_float(),
243  rotation_quat[2].as_float(), rotation_quat[3].as_float()),
244  tf::Vector3(translation[0].as_float(), translation[1].as_float(),
245  translation[2].as_float())),
246  t, source_frame);
248 
249  try {
250  tf_listener->transform_pose(target_frame, in, out);
251 
252  tf::Quaternion in_q = in.getRotation();
253  tf::Quaternion out_q = out.getRotation();
254  logger->log_debug(name(), "Transformed pose %s->%s: "
255  "T(%.2f,%.2f,%.2f) R(%.2f,%.2f,%.2f,%.2f) -> "
256  "T(%.2f,%.2f,%.2f) R(%.2f,%.2f,%.2f,%.2f)",
257  source_frame.c_str(), target_frame.c_str(),
258  in.getOrigin().x(), in.getOrigin().y(), in.getOrigin().z(),
259  in_q.x(), in_q.y(), in_q.z(), in_q.w(),
260  out.getOrigin().x(), out.getOrigin().y(), out.getOrigin().z(),
261  out_q.x(), out_q.y(), out_q.z(), out_q.w());
262 
263  CLIPS::Values rv(7, CLIPS::Value(0.));
264  rv[0] = out.getOrigin().x();
265  rv[1] = out.getOrigin().y();
266  rv[2] = out.getOrigin().z();
267  rv[3] = out_q.x();
268  rv[4] = out_q.y();
269  rv[5] = out_q.z();
270  rv[6] = out_q.w();
271  return rv;
272  } catch (Exception &e) {
273  logger->log_warn(name(), "Failed to transform pose from %s to %s: %s",
274  source_frame.c_str(), target_frame.c_str(), e.what_no_backtrace());
275  return CLIPS::Values(1, CLIPS::Value("FALSE", CLIPS::TYPE_SYMBOL));
276  }
277 }
278 
279 
280 CLIPS::Values
281 ClipsTFThread::clips_tf_quat_from_yaw(double yaw)
282 {
283  tf::Quaternion q = tf::create_quaternion_from_yaw(yaw);
284  CLIPS::Values rv(4, CLIPS::Value(0.));
285  rv[0] = q.x();
286  rv[1] = q.y();
287  rv[2] = q.z();
288  rv[3] = q.w();
289  return rv;
290 }
291 
292 
293 double
294 ClipsTFThread::clips_tf_yaw_from_quat(CLIPS::Values quat)
295 {
296  tf::Quaternion q(quat[0].as_float(), quat[1].as_float(),
297  quat[2].as_float(), quat[3].as_float());
298  return tf::get_yaw(q);
299 }
300 
301 
302 void
304 {
305 }
306 
307 
308 bool
309 ClipsTFThread::validate_time(const CLIPS::Values &time)
310 {
311  if (time.size() != 2) {
312  logger->log_warn(name(), "Invalid time: must be list of exactly two entries");
313  return false;
314  }
315  for (auto &t : time) {
316  CLIPS::Type t_type = t.type();
317  if (t_type != CLIPS::TYPE_INTEGER) {
318  logger->log_warn(name(), "Invalid time: must be list of integers");
319  return false;
320  }
321  }
322  return true;
323 }
324 
326 ClipsTFThread::convert_time(const CLIPS::Values &time)
327 {
328  if (! validate_time(time)) return fawkes::Time(0,0);
329 
330  return fawkes::Time(time[0].as_integer(), time[1].as_integer());
331 }
332 
333 
334 bool
335 ClipsTFThread::validate_point(const CLIPS::Values &point)
336 {
337  if (point.size() != 3) {
338  logger->log_warn(name(), "Invalid point: must be list of exactly three entries");
339  return false;
340  }
341  for (auto &c : point) {
342  CLIPS::Type c_type = c.type();
343  if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
344  logger->log_warn(name(), "Invalid point: must be list of floats or integers");
345  return false;
346  }
347  }
348  return true;
349 }
350 
351 bool
352 ClipsTFThread::validate_vector3(const CLIPS::Values &vector3)
353 {
354  if (vector3.size() != 3) {
355  logger->log_warn(name(), "Invalid vector: must be list of exactly three entries");
356  return false;
357  }
358  for (auto &c : vector3) {
359  CLIPS::Type c_type = c.type();
360  if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
361  logger->log_warn(name(), "Invalid vector: must be list of floats or integers");
362  return false;
363  }
364  }
365  return true;
366 }
367 
368 
369 bool
370 ClipsTFThread::validate_quat(const CLIPS::Values &quat)
371 {
372  if (quat.size() != 4) {
373  logger->log_warn(name(), "Invalid quaternion: must be list of exactly four entries");
374  return false;
375  }
376  for (auto &c : quat) {
377  CLIPS::Type c_type = c.type();
378  if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
379  logger->log_warn(name(), "Invalid quaternion: must be list of floats or integers");
380  return false;
381  }
382  }
383  return true;
384 }
Thread aspect to provide a feature to CLIPS environments.
Definition: clips_feature.h:57
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
ClipsTFThread()
Constructor.
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.
Fawkes library namespace.
void unlock() const
Unlock object mutex.
Definition: lockptr.h:255
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
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_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
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.
virtual void clips_context_init(const std::string &env_name, fawkes::LockPtr< CLIPS::Environment > &clips)
Initialize a CLIPS context to use the provided feature.
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.
Base class for exceptions in Fawkes.
Definition: exception.h:36
CLIPS feature maintainer.
Definition: clips_feature.h:41
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:686
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:133
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void finalize()
Finalize the thread.
virtual void clips_context_destroyed(const std::string &env_name)
Notification that a CLIPS environment has been destroyed.
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system...
Definition: tf.h:70
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:247
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
virtual ~ClipsTFThread()
Destructor.