00001
00002 #include <ias_drawer_executive/Geometry.h>
00003
00004 tf::TransformListener *Geometry::listener_=0;
00005
00006 void Geometry::init()
00007 {
00008 if (!listener_)
00009 listener_ = new tf::TransformListener();
00010 }
00011
00012 tf::Stamped<tf::Pose> Geometry::scaleStampedPose(const tf::Stamped<tf::Pose> &in, double scale)
00013 {
00014 tf::Stamped<tf::Pose> ret;
00015 ret.setRotation(btQuaternion::getIdentity().slerp(in.getRotation(), scale));
00016 ret.setOrigin(in.getOrigin() * scale);
00017 return ret;
00018 }
00019
00020 tf::Stamped<tf::Pose> Geometry::scaleStampedTransform(const tf::Stamped<tf::Pose> &in, double scale)
00021 {
00022 tf::Stamped<tf::Pose> ret;
00023 ret.setRotation(btQuaternion::getIdentity().slerp(in.getRotation(), scale));
00024 ret.setOrigin(in.getOrigin() * scale);
00025 return ret;
00026 }
00027
00028
00029 tf::Stamped<tf::Pose> Geometry::getPoseIn(const char target_frame[], tf::Stamped<tf::Pose>src)
00030 {
00031
00032 if (src.frame_id_ == "NO_ID_STAMPED_DEFAULT_CONSTRUCTION")
00033 {
00034 ROS_ERROR("Frame not in TF: %s", src.frame_id_.c_str());
00035 tf::Stamped<tf::Pose> pose;
00036 return pose;
00037 }
00038
00039 if (!listener_)
00040 listener_ = new tf::TransformListener();
00041
00042 tf::Stamped<tf::Pose> transform;
00043
00044 src.stamp_ = ros::Time(0);
00045
00046 listener_->waitForTransform(src.frame_id_, target_frame,
00047 ros::Time(0), ros::Duration(30.0));
00048 bool transformOk = false;
00049 while (!transformOk)
00050 {
00051 try
00052 {
00053 transformOk = true;
00054 listener_->transformPose(target_frame, src, transform);
00055 }
00056 catch (tf::TransformException ex)
00057 {
00058 ROS_ERROR("getPoseIn %s",ex.what());
00059
00060 src.stamp_ = ros::Time(0);
00061 transformOk = false;
00062 }
00063 ros::spinOnce();
00064 }
00065 return transform;
00066 }
00067
00068 void Geometry::printPose(const tf::Stamped<tf::Pose> &toolTargetPose)
00069 {
00070 ROS_INFO("%f %f %f %f %f %f %f %s\n",
00071 toolTargetPose.getOrigin().x(), toolTargetPose.getOrigin().y(), toolTargetPose.getOrigin().z(),toolTargetPose.getRotation().x(),
00072 toolTargetPose.getRotation().y(),toolTargetPose.getRotation().z(),toolTargetPose.getRotation().w(),toolTargetPose.frame_id_.c_str());
00073 }
00074
00075
00076
00077 tf::Stamped<tf::Pose> Geometry::getPose(const char target_frame[],const char lookup_frame[])
00078 {
00079
00080 init();
00081
00082 ros::Rate rate(100.0);
00083
00084 tf::StampedTransform transform;
00085 bool transformOk = false;
00086
00087 listener_->waitForTransform(target_frame, lookup_frame, ros::Time(0), ros::Duration(0.5));
00088 while (ros::ok() && (!transformOk))
00089 {
00090 transformOk = true;
00091 try
00092 {
00093 listener_->lookupTransform(target_frame, lookup_frame,ros::Time(0), transform);
00094 }
00095 catch (tf::TransformException ex)
00096 {
00097 ROS_ERROR("getPose: tf::TransformException ex.what()='%s'",ex.what());
00098 transformOk = false;
00099 listener_->waitForTransform(target_frame, lookup_frame, ros::Time(0), ros::Duration(0.5));
00100 }
00101 if (!transformOk)
00102 rate.sleep();
00103 }
00104 tf::Stamped<tf::Pose> ret;
00105 ret.frame_id_ = transform.frame_id_;
00106 ret.stamp_ = transform.stamp_;
00107 ret.setOrigin(transform.getOrigin());
00108 ret.setRotation(transform.getRotation());
00109
00110 return ret;
00111 }
00112
00113
00114
00115 tf::Stamped<tf::Pose> Geometry::getRelativeTransform(const char source_frameid[], const char target_frameid[])
00116 {
00117 tf::StampedTransform transform;
00118
00119 init();
00120
00121 listener_->waitForTransform(source_frameid, target_frameid,
00122 ros::Time(), ros::Duration(10.0));
00123
00124 try
00125 {
00126 listener_->lookupTransform(std::string(source_frameid), std::string(target_frameid), ros::Time(), transform);
00127 }
00128 catch (tf::TransformException ex)
00129 {
00130 ROS_ERROR("getTransformIn %s",ex.what());
00131 }
00132
00133 tf::Stamped<tf::Pose> ret;
00134 ret.frame_id_ = transform.frame_id_;
00135 ret.stamp_ = transform.stamp_;
00136 ret.setOrigin(transform.getOrigin());
00137 ret.setRotation(transform.getRotation());
00138
00139 return ret;
00140 }
00141
00142 tf::Stamped<tf::Pose> Geometry::getTransform(const char baseframe[], const char toolframe[])
00143 {
00144
00145 ros::Rate rate(100.0);
00146
00147 tf::StampedTransform transform;
00148 bool transformOk = false;
00149 while (ros::ok() && (!transformOk))
00150 {
00151 transformOk = true;
00152 try
00153 {
00154 listener_->lookupTransform(baseframe, toolframe,ros::Time(0), transform);
00155 }
00156 catch (tf::TransformException ex)
00157 {
00158 ROS_ERROR("getTransform %s",ex.what());
00159 transformOk = false;
00160 }
00161 rate.sleep();
00162 }
00163 tf::Stamped<tf::Pose> ret;
00164 ret.frame_id_ = transform.frame_id_;
00165 ret.stamp_ = transform.stamp_;
00166 ret.setOrigin(transform.getOrigin());
00167 ret.setRotation(transform.getRotation());
00168
00169 return ret;
00170 }
00171
00172 tf::Stamped<tf::Pose> Geometry::rotateAroundBaseAxis(tf::Stamped<tf::Pose> toolPose, double r_x,double r_y,double r_z)
00173 {
00174 tf::Stamped<tf::Pose> toolRotPlus;
00175 toolRotPlus.setOrigin(btVector3(0,0,0));
00176 toolRotPlus.setRotation(btQuaternion(r_x,r_y,r_z));
00177 btVector3 orig = toolPose.getOrigin();
00178 toolPose.setOrigin(btVector3(0,0,0));
00179 toolRotPlus *= toolPose;
00180 toolPose.setOrigin(orig);
00181 toolPose.setRotation(toolRotPlus.getRotation());
00182 return toolPose;
00183 }
00184
00185 tf::Stamped<tf::Pose> Geometry::rotateAroundToolframeAxis(tf::Stamped<tf::Pose> toolPose, double r_x,double r_y,double r_z)
00186 {
00187 tf::Stamped<tf::Pose> toolRotPlus;
00188 toolRotPlus.setOrigin(btVector3(0,0,0));
00189 toolRotPlus.setRotation(btQuaternion(r_x,r_y,r_z));
00190 btVector3 orig = toolPose.getOrigin();
00191 toolPose *= toolRotPlus;
00192 return toolPose;
00193 }
00194
00195
00196 tf::Stamped<tf::Pose> Geometry::rotateAroundPose(tf::Stamped<tf::Pose> toolPose, tf::Stamped<tf::Pose> pivot, double r_x, double r_y, double r_z)
00197 {
00198 btTransform curr = toolPose;
00199 btTransform pivo = pivot;
00200
00201 curr = pivo.inverseTimes(curr);
00202
00203 btQuaternion qa;
00204 qa.setEulerZYX(r_z,r_y,r_x);
00205
00206 btTransform rot;
00207 rot.setOrigin(btVector3(0,0,0));
00208 rot.setRotation(qa);
00209 curr = rot * curr;
00210 curr = pivo * curr;
00211
00212 tf::Stamped<tf::Pose> act;
00213 act.frame_id_ = toolPose.frame_id_;
00214 act.setOrigin(curr.getOrigin());
00215 act.setRotation(curr.getRotation());
00216
00217 return act;
00218 }
00219
00220
00221 tf::Stamped<tf::Pose> Geometry::rotateAroundPose(tf::Stamped<tf::Pose> toolPose, tf::Stamped<tf::Pose> pivot, btQuaternion qa)
00222 {
00223 btTransform curr = toolPose;
00224 btTransform pivo = pivot;
00225
00226 curr = pivo.inverseTimes(curr);
00227
00228 btTransform rot;
00229 rot.setOrigin(btVector3(0,0,0));
00230 rot.setRotation(qa);
00231 curr = rot * curr;
00232 curr = pivo * curr;
00233
00234 tf::Stamped<tf::Pose> act;
00235 act.frame_id_ = toolPose.frame_id_;
00236 act.setOrigin(curr.getOrigin());
00237 act.setRotation(curr.getRotation());
00238
00239 return act;
00240 }
00241
00242
00243 tf::Stamped<tf::Pose> Geometry::approach(tf::Stamped<tf::Pose> toolPose, double dist)
00244 {
00245 tf::Stamped<tf::Pose> appr;
00246 appr.stamp_ = ros::Time();
00247 appr.setOrigin(btVector3(- dist,0,0));
00248 appr.setRotation(btQuaternion(0,0,0,1));
00249 tf::Stamped<tf::Pose> ret;
00250 ret = toolPose;
00251 ret *= appr;
00252 return ret;
00253 }
00254
00255
00256 tf::Stamped<tf::Pose> Geometry::make_pose(double x, double y, double z, double ox, double oy, double oz, double ow, const char target_frame[])
00257 {
00258 tf::Stamped<tf::Pose> toolTargetPose;
00259
00260 toolTargetPose.frame_id_ = target_frame;
00261 toolTargetPose.stamp_ = ros::Time(0);
00262 toolTargetPose.setOrigin(btVector3( x,y,z));
00263 toolTargetPose.setRotation(btQuaternion(ox,oy,oz,ow));
00264
00265 return toolTargetPose;
00266 }
00267
00268 tf::Stamped<tf::Pose> Geometry::make_pose(const btTransform &trans, const char target_frame[])
00269 {
00270 tf::Stamped<tf::Pose> toolTargetPose;
00271 toolTargetPose.frame_id_ = target_frame;
00272 toolTargetPose.stamp_ = ros::Time(0);
00273 toolTargetPose.setOrigin(trans.getOrigin());
00274 toolTargetPose.setRotation(trans.getRotation());
00275
00276 return toolTargetPose;
00277 }
00278
00279 tf::Stamped<tf::Pose> Geometry::getRel(const tf::Stamped<tf::Pose> &root_frame, const tf::Stamped<tf::Pose> &relative_pose)
00280 {
00281 btTransform rootbt;
00282 rootbt.setOrigin(root_frame.getOrigin());
00283 rootbt.setRotation(root_frame.getRotation());
00284 btTransform relativebt;
00285 relativebt.setOrigin(relative_pose.getOrigin());
00286 relativebt.setRotation(relative_pose.getRotation());
00287
00288 btTransform retbt = rootbt * relativebt;
00289
00290 tf::Stamped<tf::Pose> ret = root_frame;
00291 ret.setOrigin(retbt.getOrigin());
00292 ret.setRotation(retbt.getRotation());
00293
00294 return ret;
00295 }
00296
00297
00298 tf::Stamped<tf::Pose> Geometry::getRelInBase(const tf::Stamped<tf::Pose> &root_frame, const btVector3 &dist)
00299 {
00300 tf::Stamped<tf::Pose> ret = root_frame;
00301 ret = getPoseIn("/base_link",ret);
00302 ret.getOrigin() += dist;
00303 ret = getPoseIn(root_frame.frame_id_.c_str(),ret);
00304 return ret;
00305 }
00306
00307
00308 void Geometry::printPose(const char title[], tf::Stamped<tf::Pose> pose)
00309 {
00310 ROS_INFO("[ rosrun tf static_transform_publisher %f %f %f %f %f %f %f %s %s 100 ]", pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()
00311 , pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w(), pose.frame_id_.c_str(), title);
00312 }
