22 #include "clips_tf_thread.h" 62 envs_[env_name] = clips;
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)));
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)));
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)));
101 envs_.erase(env_name);
106 ClipsTFThread::clips_tf_frame_exists(std::string frame_id)
114 ClipsTFThread::clips_tf_can_transform(std::string target_frame, std::string source_frame,
117 if (! validate_time(time)) {
118 return CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL);
123 ?
"TRUE" :
"FALSE", CLIPS::TYPE_SYMBOL);
128 ClipsTFThread::clips_tf_transform_point(std::string target_frame, std::string source_frame,
129 CLIPS::Values time, CLIPS::Values point)
131 if (! (validate_time(time) && validate_point(point))) {
132 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
137 in(tf::Point(point[0].as_float(), point[1].as_float(), point[2].as_float()),
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());
148 CLIPS::Values rv(3, CLIPS::Value(0.));
156 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
161 ClipsTFThread::clips_tf_transform_vector( std::string target_frame, std::string source_frame,
162 CLIPS::Values time, CLIPS::Values vector3)
164 if (! (validate_time(time) && validate_vector3(vector3))) {
165 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
170 in(tf::Vector3(vector3[0].as_float(), vector3[1].as_float(), vector3[2].as_float()),
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());
181 CLIPS::Values rv(3, CLIPS::Value(0.));
189 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
194 ClipsTFThread::clips_tf_transform_quaternion(std::string target_frame, std::string source_frame,
195 CLIPS::Values time, CLIPS::Values quat)
197 if (! (validate_time(time) && validate_quat(quat))) {
198 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
203 in(tf::Quaternion(quat[0].as_float(), quat[1].as_float(),
204 quat[2].as_float(), quat[3].as_float()),
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());
216 CLIPS::Values rv(4, CLIPS::Value(0.));
225 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
231 ClipsTFThread::clips_tf_transform_pose(std::string target_frame, std::string source_frame,
233 CLIPS::Values translation, CLIPS::Values rotation_quat)
235 if (! (validate_time(time) && validate_vector3(translation) && validate_quat(rotation_quat)))
237 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
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())),
252 tf::Quaternion in_q = in.getRotation();
253 tf::Quaternion out_q = out.getRotation();
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());
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();
275 return CLIPS::Values(1, CLIPS::Value(
"FALSE", CLIPS::TYPE_SYMBOL));
281 ClipsTFThread::clips_tf_quat_from_yaw(
double yaw)
283 tf::Quaternion q = tf::create_quaternion_from_yaw(yaw);
284 CLIPS::Values rv(4, CLIPS::Value(0.));
294 ClipsTFThread::clips_tf_yaw_from_quat(CLIPS::Values quat)
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);
309 ClipsTFThread::validate_time(
const CLIPS::Values &time)
311 if (time.size() != 2) {
315 for (
auto &t : time) {
316 CLIPS::Type t_type = t.type();
317 if (t_type != CLIPS::TYPE_INTEGER) {
326 ClipsTFThread::convert_time(
const CLIPS::Values &time)
330 return fawkes::Time(time[0].as_integer(), time[1].as_integer());
335 ClipsTFThread::validate_point(
const CLIPS::Values &point)
337 if (point.size() != 3) {
341 for (
auto &c : point) {
342 CLIPS::Type c_type = c.type();
343 if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
352 ClipsTFThread::validate_vector3(
const CLIPS::Values &vector3)
354 if (vector3.size() != 3) {
358 for (
auto &c : vector3) {
359 CLIPS::Type c_type = c.type();
360 if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
370 ClipsTFThread::validate_quat(
const CLIPS::Values &quat)
372 if (quat.size() != 4) {
376 for (
auto &c : quat) {
377 CLIPS::Type c_type = c.type();
378 if (c_type != CLIPS::TYPE_FLOAT && c_type != CLIPS::TYPE_INTEGER) {
Thread aspect to provide a feature to CLIPS environments.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
ClipsTFThread()
Constructor.
Fawkes library namespace.
void unlock() const
Unlock object mutex.
A class for handling time.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
virtual void clips_context_init(const std::string &env_name, fawkes::LockPtr< CLIPS::Environment > &clips)
Initialize a CLIPS context to use the provided feature.
Base class for exceptions in Fawkes.
CLIPS feature maintainer.
const char * name() const
Get name of thread.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
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.
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.
void lock() const
Lock access to the encapsulated object.
virtual ~ClipsTFThread()
Destructor.