22 #include "clips_tf_thread.h"
63 envs_[env_name] = clips;
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)));
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)));
85 sigc::slot<CLIPS::Values, std::string, std::string, CLIPS::Values, CLIPS::Values>(
86 sigc::mem_fun(*
this, &ClipsTFThread::clips_tf_transform_point)));
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)));
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)));
98 slot<CLIPS::Values, std::string, std::string, CLIPS::Values, CLIPS::Values, CLIPS::Values>(
99 sigc::mem_fun(*
this, &ClipsTFThread::clips_tf_transform_pose)));
107 envs_.erase(env_name);
112 ClipsTFThread::clips_tf_frame_exists(std::string frame_id)
118 ClipsTFThread::clips_tf_can_transform(std::string target_frame,
119 std::string source_frame,
122 if (!validate_time(time)) {
123 return CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL);
132 ClipsTFThread::clips_tf_transform_point(std::string target_frame,
133 std::string source_frame,
137 if (!(validate_time(time) && validate_point(point))) {
138 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
143 tf::Point(point[0].as_float(), point[1].as_float(), point[2].as_float()), t, source_frame);
151 "Transformed point %s->%s: (%.2f,%.2f,%.2f) -> (%.2f,%.2f,%.2f)",
152 source_frame.c_str(),
153 target_frame.c_str(),
161 CLIPS::Values rv(3, CLIPS::Value(0.));
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));
177 ClipsTFThread::clips_tf_transform_vector(std::string target_frame,
178 std::string source_frame,
180 CLIPS::Values vector3)
182 if (!(validate_time(time) && validate_vector3(vector3))) {
183 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
188 vector3[1].as_float(),
189 vector3[2].as_float()),
199 "Transformed vector %s->%s: (%.2f,%.2f,%.2f) -> (%.2f,%.2f,%.2f)",
200 source_frame.c_str(),
201 target_frame.c_str(),
209 CLIPS::Values rv(3, CLIPS::Value(0.));
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));
225 ClipsTFThread::clips_tf_transform_quaternion(std::string target_frame,
226 std::string source_frame,
230 if (!(validate_time(time) && validate_quat(quat))) {
231 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
236 tf::Quaternion(quat[0].as_float(), quat[1].as_float(), quat[2].as_float(), quat[3].as_float()),
246 "Transformed quaternion %s->%s: "
247 "(%.2f,%.2f,%.2f,%.2f) -> (%.2f,%.2f,%.2f,%.2f)",
248 source_frame.c_str(),
249 target_frame.c_str(),
259 CLIPS::Values rv(4, CLIPS::Value(0.));
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));
276 ClipsTFThread::clips_tf_transform_pose(std::string target_frame,
277 std::string source_frame,
279 CLIPS::Values translation,
280 CLIPS::Values rotation_quat)
282 if (!(validate_time(time) && validate_vector3(translation) && validate_quat(rotation_quat))) {
283 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
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())),
301 tf::Quaternion in_q = in.getRotation();
302 tf::Quaternion out_q = out.getRotation();
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(),
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();
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));
345 ClipsTFThread::clips_tf_quat_from_yaw(
double yaw)
347 tf::Quaternion q = tf::create_quaternion_from_yaw(yaw);
348 CLIPS::Values rv(4, CLIPS::Value(0.));
357 ClipsTFThread::clips_tf_yaw_from_quat(CLIPS::Values quat)
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);
369 ClipsTFThread::validate_time(
const CLIPS::Values &time)
371 if (time.size() != 2) {
375 for (
auto &t : time) {
376 CLIPS::Type t_type = t.type();
377 if (t_type != CLIPS::TYPE_INTEGER) {
386 ClipsTFThread::convert_time(
const CLIPS::Values &time)
388 if (!validate_time(time))
391 return fawkes::Time(time[0].as_integer(), time[1].as_integer());
395 ClipsTFThread::validate_point(
const CLIPS::Values &point)
397 if (point.size() != 3) {
401 for (
auto &c : point) {
402 CLIPS::Type c_type = c.type();
403 if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
412 ClipsTFThread::validate_vector3(
const CLIPS::Values &vector3)
414 if (vector3.size() != 3) {
418 for (
auto &c : vector3) {
419 CLIPS::Type c_type = c.type();
420 if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
429 ClipsTFThread::validate_quat(
const CLIPS::Values &quat)
431 if (quat.size() != 4) {
435 for (
auto &c : quat) {
436 CLIPS::Type c_type = c.type();
437 if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
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.
CLIPS feature maintainer.
Configuration * config
This is the Configuration member used to access the configuration.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
Base class for exceptions in Fawkes.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
void lock() const
Lock access to the encapsulated object.
void unlock() const
Unlock object mutex.
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.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
A class for handling time.
Wrapper class to add time stamp and frame ID to base types.
Fawkes library namespace.