Geometry.cpp
Go to the documentation of this file.
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     //this shouldnt be here TODO
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             // dirty:
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     //tf::TransformListener listener;
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     //tf::TransformListener listener;
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 }


ias_drawer_executive
Author(s): Thomas Ruehr
autogenerated on Mon Oct 6 2014 08:59:21