00001
00002
00003
00004
00005
00006
00007
00008 #include "tidyup_utils/transformer.h"
00009
00010 Transformer* Transformer::instance = NULL;
00011
00012 Transformer::Transformer() :
00013 robot_model("/robot_description")
00014 {
00015 }
00016
00017 Transformer::~Transformer()
00018 {
00019 }
00020
00021 std::string Transformer::relative_frame(const std::string& frame_id) const
00022 {
00023 if (frame_id[0] != '/')
00024 {
00025 return frame_id;
00026 }
00027 return frame_id.substr(1, frame_id.size());
00028 }
00029
00030 bool Transformer::getTransform(const std::string& to_frame,
00031 const std::string& from_frame,
00032 const arm_navigation_msgs::RobotState& robotState,
00033 tf::Transform& transform)
00034 {
00035 planning_models::KinematicState kstate(robot_model.getKinematicModel());
00036 if (!planning_environment::setRobotStateAndComputeTransforms(robotState, kstate))
00037 {
00038 ROS_ERROR("Unable to transform robot state to kinematic state");
00039 return false;
00040 }
00041
00042 ROS_INFO("transforming from %s to %s", from_frame.c_str(), to_frame.c_str());
00043
00044 std::vector<geometry_msgs::TransformStamped> transforms;
00045 tf::Transform global_to_from;
00046 if (!getWorldTransform(from_frame, kstate, global_to_from))
00047 {
00048 ROS_ERROR("Unable to find frame_id %s in kinematic state", from_frame.c_str());
00049 return false;
00050 }
00051 tf::Transform global_to_to;
00052 if (!getWorldTransform(to_frame, kstate, global_to_to))
00053 {
00054 ROS_ERROR("Unable to find frame_id %s in kinematic state", to_frame.c_str());
00055 return false;
00056 }
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066 transform = (global_to_to.inverse()) * global_to_from;
00067 return true;
00068 }
00069
00070 bool Transformer::getWorldTransform(const std::string& frame_id,
00071 const planning_models::KinematicState& state,
00072 tf::Transform &transform)
00073 {
00074 if (!frame_id.compare(state.getKinematicModel()->getRoot()->getParentFrameId()))
00075 {
00076
00077 transform.setIdentity();
00078 return true;
00079 }
00080
00081 if (!frame_id.compare(state.getKinematicModel()->getRoot()->getChildFrameId()))
00082 {
00083 transform = state.getRootTransform();
00084 return true;
00085 }
00086
00087 const planning_models::KinematicState::LinkState *link = state.getLinkState(frame_id);
00088 if (!link)
00089 {
00090 ROS_ERROR("Unable to find link %s in kinematic state", frame_id.c_str());
00091 return false;
00092 }
00093
00094 transform = link->getGlobalLinkTransform();
00095 return true;
00096 }
00097
00098 bool Transformer::transform(const std::string& new_frame,
00099 const std::string& old_frame,
00100 const arm_navigation_msgs::RobotState& robotState,
00101 geometry_msgs::Point& point)
00102 {
00103 tf::Vector3 btPoint(point.x, point.y, point.z);
00104 tf::Transform trans;
00105 if (instance == NULL)
00106 instance = new Transformer();
00107 if (instance->getTransform(new_frame, old_frame, robotState, trans))
00108 {
00109 btPoint = trans * btPoint;
00110 point.x = btPoint.x();
00111 point.y = btPoint.y();
00112 point.z = btPoint.z();
00113 return true;
00114 }
00115 return false;
00116 }
00117
00118 bool Transformer::transform(const std::string& new_frame,
00119 const std::string& old_frame,
00120 const arm_navigation_msgs::RobotState& robotState,
00121 geometry_msgs::Quaternion& quaternion)
00122 {
00123 tf::Quaternion bt_Quaternion(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
00124 tf::Transform trans;
00125 if (instance == NULL)
00126 instance = new Transformer();
00127 if (instance->getTransform(new_frame, old_frame, robotState, trans))
00128 {
00129 bt_Quaternion = trans * bt_Quaternion;
00130 quaternion.x = bt_Quaternion.x();
00131 quaternion.y = bt_Quaternion.y();
00132 quaternion.z = bt_Quaternion.z();
00133 quaternion.w = bt_Quaternion.w();
00134 return true;
00135 }
00136 return false;
00137 }
00138
00139 bool Transformer::transform(const std::string& new_frame,
00140 const std::string& old_frame,
00141 const arm_navigation_msgs::RobotState& robotState,
00142 geometry_msgs::Pose& pose)
00143 {
00144 geometry_msgs::Point& point = pose.position;
00145 geometry_msgs::Quaternion& quaternion = pose.orientation;
00146 tf::Vector3 btPoint(point.x, point.y, point.z);
00147 tf::Quaternion bt_Quaternion(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
00148 tf::Transform trans;
00149 if (instance == NULL)
00150 instance = new Transformer();
00151 if (instance->getTransform(new_frame, old_frame, robotState, trans))
00152 {
00153 btPoint = trans * btPoint;
00154 point.x = btPoint.x();
00155 point.y = btPoint.y();
00156 point.z = btPoint.z();
00157 bt_Quaternion = trans * bt_Quaternion;
00158 quaternion.x = bt_Quaternion.x();
00159 quaternion.y = bt_Quaternion.y();
00160 quaternion.z = bt_Quaternion.z();
00161 quaternion.w = bt_Quaternion.w();
00162 return true;
00163 }
00164 return false;
00165 }
00166
00167 bool Transformer::transform(const std::string& new_frame,
00168 const std::string& old_frame,
00169 const arm_navigation_msgs::RobotState& robotState,
00170 geometry_msgs::PointStamped& point)
00171 {
00172 if (transform(new_frame, old_frame, robotState, point.point))
00173 {
00174 point.header.frame_id = new_frame;
00175 return true;
00176 }
00177 return false;
00178 }
00179
00180 bool Transformer::transform(const std::string& new_frame,
00181 const std::string& old_frame,
00182 const arm_navigation_msgs::RobotState& robotState,
00183 geometry_msgs::QuaternionStamped& quaternion)
00184 {
00185 if (transform(new_frame, old_frame, robotState, quaternion.quaternion))
00186 {
00187 quaternion.header.frame_id = new_frame;
00188 return true;
00189 }
00190 return false;
00191 }
00192
00193 bool Transformer::transform(const std::string& new_frame,
00194 const std::string& old_frame,
00195 const arm_navigation_msgs::RobotState& robotState,
00196 geometry_msgs::PoseStamped& pose)
00197 {
00198 if (transform(new_frame, old_frame, robotState, pose.pose))
00199 {
00200 pose.header.frame_id = new_frame;
00201 return true;
00202 }
00203 return false;
00204 }
00205
00206 bool Transformer::transform(const std::string& new_frame,
00207 const arm_navigation_msgs::RobotState& robotState,
00208 arm_navigation_msgs::Constraints& constraint)
00209 {
00210 bool ok = true;
00211 for(std::vector <arm_navigation_msgs::PositionConstraint>::iterator it = constraint.position_constraints.begin();
00212 it != constraint.position_constraints.end(); it++)
00213 {
00214 ok &= transform(new_frame, it->header.frame_id, robotState, it->position);
00215 ok &= transform(new_frame, it->header.frame_id, robotState, it->constraint_region_orientation);
00216 it->header.frame_id = new_frame;
00217 }
00218 for(std::vector <arm_navigation_msgs::OrientationConstraint>::iterator it = constraint.orientation_constraints.begin();
00219 it != constraint.orientation_constraints.end(); it++)
00220 {
00221 ok &= transform(new_frame, it->header.frame_id, robotState, it->orientation);
00222 it->header.frame_id = new_frame;
00223 }
00224 for(std::vector <arm_navigation_msgs::VisibilityConstraint>::iterator it = constraint.visibility_constraints.begin();
00225 it != constraint.visibility_constraints.end(); it++)
00226 {
00227 ok &= transform(new_frame, it->header.frame_id, robotState, it->target);
00228 }
00229 return ok;
00230 }