00001 #include "wam_tcp_ik_alg_node.h"
00002
00003 WamTcpIkAlgNode::WamTcpIkAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<WamTcpIkAlgorithm>()
00005 {
00006
00007 public_node_handle_.param<std::string>("robot_base", robot_base_str_, "robot_base_frame");
00008 public_node_handle_.param<std::string>("robot_tcp", robot_tcp_str_, "robot_tcp_frame");
00009 public_node_handle_.param<std::string>("tool_tcp", tool_tcp_str_, "tool_tcp_frame");
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 get_ik_server_ = this->public_node_handle_.advertiseService( "get_wam_ik", &WamTcpIkAlgNode::get_ikCallback, this);
00020 get_robot_pose_server_ = this->public_node_handle_.advertiseService( "get_wam_robot_pose", &WamTcpIkAlgNode::get_robotPoseCallback, this);
00021
00022
00023 get_ik_client_ = this->public_node_handle_.serviceClient<iri_wam_common_msgs::wamInverseKinematics>("wamik");
00024
00025
00026
00027
00028 }
00029
00030 WamTcpIkAlgNode::~WamTcpIkAlgNode(void)
00031 {
00032
00033 }
00034
00035 void WamTcpIkAlgNode::mainNodeThread(void)
00036 {
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054 }
00055
00056
00057
00058
00059 bool WamTcpIkAlgNode::get_ikCallback(iri_wam_common_msgs::wamInverseKinematics::Request &req, iri_wam_common_msgs::wamInverseKinematics::Response &res)
00060 {
00061 ROS_INFO("WamTcpIkAlgNode::get_ikCallback: New Request Received!");
00062
00063
00064
00065
00066
00067
00068 try{
00069 ros::Time now = ros::Time::now();
00070 ros::Duration interval = ros::Duration(1.0);
00071 if(!listener_.waitForTransform(tool_tcp_str_, robot_tcp_str_, now, interval)){
00072 ROS_ERROR("Timeout while waiting for transform between frames %s and %s ", tool_tcp_str_.c_str(), robot_tcp_str_.c_str());
00073 }
00074 listener_.lookupTransform(tool_tcp_str_, robot_tcp_str_, now, tcp_H_wam7_);
00075 }catch (tf::TransformException ex){
00076 ROS_ERROR("lookup transform error: %s", ex.what());
00077 return false;
00078 }
00079
00080
00081 ROS_INFO("[WamTcpIkAlgNode] %s_H_7 Pose (x, y, z, qx, qy, qz, qw): [ %f, %f, %f, %f, %f, %f, %f ]",
00082 tool_tcp_str_.c_str(),
00083 tcp_H_wam7_.getOrigin().x(),
00084 tcp_H_wam7_.getOrigin().y(),
00085 tcp_H_wam7_.getOrigin().z(),
00086 tcp_H_wam7_.getRotation().x(),
00087 tcp_H_wam7_.getRotation().y(),
00088 tcp_H_wam7_.getRotation().z(),
00089 tcp_H_wam7_.getRotation().w());
00090
00091
00092 ROS_INFO("[WamTcpIkAlgNode] Received Pose from frame %s (x, y, z, qx, qy, qz, qw): [ %f, %f, %f, %f, %f, %f, %f ]",
00093 req.pose.header.frame_id.c_str(),
00094 req.pose.pose.position.x,
00095 req.pose.pose.position.y,
00096 req.pose.pose.position.z,
00097 req.pose.pose.orientation.x,
00098 req.pose.pose.orientation.y,
00099 req.pose.pose.orientation.z,
00100 req.pose.pose.orientation.w);
00101
00102
00103 tf::Quaternion world_quat_tcp( req.pose.pose.orientation.x, req.pose.pose.orientation.y, req.pose.pose.orientation.z, req.pose.pose.orientation.w);
00104 tf::Vector3 world_pos_tcp( req.pose.pose.position.x, req.pose.pose.position.y, req.pose.pose.position.z);
00105 tf::Transform received_pose( world_quat_tcp, world_pos_tcp);
00106
00107
00108
00109
00110
00111
00112 try{
00113 ros::Time now = ros::Time::now();
00114 ros::Duration interval = ros::Duration(1.0);
00115 if(!listener_.waitForTransform(robot_base_str_, req.pose.header.frame_id, now, interval)){
00116 ROS_ERROR("Timeout while waiting for transform between frames %s and %s ", robot_base_str_.c_str(), req.pose.header.frame_id.c_str());
00117 }
00118 listener_.lookupTransform(robot_base_str_, req.pose.header.frame_id, now, world_H_wam7_);
00119 }catch (tf::TransformException ex){
00120 ROS_ERROR("lookup transform error: %s", ex.what());
00121 return false;
00122 }
00123
00124 world_H_wam7_ *= received_pose;
00125 world_H_wam7_ *= tcp_H_wam7_;
00126
00127 base_pose_msg_.request.pose.header.frame_id = robot_base_str_;
00128 base_pose_msg_.request.pose.pose.position.x = world_H_wam7_.getOrigin().x();
00129 base_pose_msg_.request.pose.pose.position.y = world_H_wam7_.getOrigin().y();
00130 base_pose_msg_.request.pose.pose.position.z = world_H_wam7_.getOrigin().z();
00131 base_pose_msg_.request.pose.pose.orientation.x = world_H_wam7_.getRotation().x();
00132 base_pose_msg_.request.pose.pose.orientation.y = world_H_wam7_.getRotation().y();
00133 base_pose_msg_.request.pose.pose.orientation.z = world_H_wam7_.getRotation().z();
00134 base_pose_msg_.request.pose.pose.orientation.w = world_H_wam7_.getRotation().w();
00135
00136 ROS_INFO("[WamTcpIkAlgNode] 0_H_7 Pose (x, y, z, qx, qy, qz, qw): [ %f, %f, %f, %f, %f, %f, %f ]",
00137 world_H_wam7_.getOrigin().x(),
00138 world_H_wam7_.getOrigin().y(),
00139 world_H_wam7_.getOrigin().z(),
00140 world_H_wam7_.getRotation().x(),
00141 world_H_wam7_.getRotation().y(),
00142 world_H_wam7_.getRotation().z(),
00143 world_H_wam7_.getRotation().w());
00144
00145 bool result;
00146 if(get_ik_client_.call(base_pose_msg_)){
00147 res.joints.position.resize(7);
00148 for(int ii=0; ii < 7; ++ii)
00149 res.joints.position[ii] = base_pose_msg_.response.joints.position[ii];
00150 ROS_INFO("[WamTcpIkAlgNode] Joints readings: (%f, %f, %f, %f, %f, %f, %f)",
00151 base_pose_msg_.response.joints.position[0],
00152 base_pose_msg_.response.joints.position[1],
00153 base_pose_msg_.response.joints.position[2],
00154 base_pose_msg_.response.joints.position[3],
00155 base_pose_msg_.response.joints.position[4],
00156 base_pose_msg_.response.joints.position[5],
00157 base_pose_msg_.response.joints.position[6] );
00158 result = true;
00159 }else{
00160 ROS_ERROR("Failed to call service %s", get_ik_client_.getService().c_str());
00161 result = false;
00162 }
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180 return result;
00181 }
00182
00183 bool WamTcpIkAlgNode::get_robotPoseCallback(iri_wam_common_msgs::wamGetRobotPoseFromToolPose::Request &req, iri_wam_common_msgs::wamGetRobotPoseFromToolPose::Response &res)
00184 {
00185
00186
00187
00188
00189
00190
00191
00192 try{
00193 ros::Time now = ros::Time::now();
00194 ros::Duration interval = ros::Duration(1.0);
00195 if(!listener_.waitForTransform(tool_tcp_str_, robot_tcp_str_, now, interval)){
00196 ROS_ERROR("Timeout while waiting for transform between frames %s and %s", tool_tcp_str_.c_str(), robot_tcp_str_.c_str());
00197 }
00198 listener_.lookupTransform(tool_tcp_str_, robot_tcp_str_, now, tcp_H_wam7_);
00199 }catch (tf::TransformException ex){
00200 ROS_ERROR("lookup transform error: %s", ex.what());
00201 return false;
00202 }
00203
00204 ROS_INFO("[WamTcpIkAlgNode] %s_H_7 Pose (x, y, z, qx, qy, qz, qw): [ %f, %f, %f, %f, %f, %f, %f ]",tool_tcp_str_.c_str(),
00205 tcp_H_wam7_.getOrigin().x(),
00206 tcp_H_wam7_.getOrigin().y(),
00207 tcp_H_wam7_.getOrigin().z(),
00208 tcp_H_wam7_.getRotation().x(),
00209 tcp_H_wam7_.getRotation().y(),
00210 tcp_H_wam7_.getRotation().z(),
00211 tcp_H_wam7_.getRotation().w());
00212
00213
00214 ROS_INFO("[WamTcpIkAlgNode] Received Pose from frame %s (x, y, z, qx, qy, qz, qw): [ %f, %f, %f, %f, %f, %f, %f ]",
00215 req.tool_pose.header.frame_id.c_str(),
00216 req.tool_pose.pose.position.x,
00217 req.tool_pose.pose.position.y,
00218 req.tool_pose.pose.position.z,
00219 req.tool_pose.pose.orientation.x,
00220 req.tool_pose.pose.orientation.y,
00221 req.tool_pose.pose.orientation.z,
00222 req.tool_pose.pose.orientation.w);
00223
00224
00225 tf::Quaternion world_quat_tcp( req.tool_pose.pose.orientation.x, req.tool_pose.pose.orientation.y, req.tool_pose.pose.orientation.z, req.tool_pose.pose.orientation.w);
00226 tf::Vector3 world_pos_tcp( req.tool_pose.pose.position.x, req.tool_pose.pose.position.y, req.tool_pose.pose.position.z);
00227 tf::Transform received_pose( world_quat_tcp, world_pos_tcp);
00228
00229
00230 try{
00231 ros::Time now = ros::Time::now();
00232 ros::Duration interval = ros::Duration(1.0);
00233 if(!listener_.waitForTransform(robot_base_str_, req.tool_pose.header.frame_id, now, interval)){
00234 ROS_ERROR("Timeout while waiting for transform between frames %s and %s ", robot_base_str_.c_str(), req.tool_pose.header.frame_id.c_str());
00235 }
00236 listener_.lookupTransform(robot_base_str_, req.tool_pose.header.frame_id, now, world_H_wam7_);
00237 }catch (tf::TransformException ex){
00238 ROS_ERROR("lookup transform error: %s", ex.what());
00239 return false;
00240 }
00241
00242 world_H_wam7_ *= received_pose;
00243 world_H_wam7_ *= tcp_H_wam7_;
00244
00245 ROS_INFO("[WamTcpIkAlgNode] 0_H_7 Pose (x, y, z, qx, qy, qz, qw): [ %f, %f, %f, %f, %f, %f, %f ]",
00246 world_H_wam7_.getOrigin().x(),
00247 world_H_wam7_.getOrigin().y(),
00248 world_H_wam7_.getOrigin().z(),
00249 world_H_wam7_.getRotation().x(),
00250 world_H_wam7_.getRotation().y(),
00251 world_H_wam7_.getRotation().z(),
00252 world_H_wam7_.getRotation().w());
00253
00254 res.robot_pose.header.frame_id = robot_base_str_;
00255 res.robot_pose.pose.position.x = world_H_wam7_.getOrigin().x();
00256 res.robot_pose.pose.position.y = world_H_wam7_.getOrigin().y();
00257 res.robot_pose.pose.position.z = world_H_wam7_.getOrigin().z();
00258 res.robot_pose.pose.orientation.x = world_H_wam7_.getRotation().x();
00259 res.robot_pose.pose.orientation.y = world_H_wam7_.getRotation().y();
00260 res.robot_pose.pose.orientation.z = world_H_wam7_.getRotation().z();
00261 res.robot_pose.pose.orientation.w = world_H_wam7_.getRotation().w();
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278 return true;
00279 }
00280
00281
00282
00283
00284
00285
00286 void WamTcpIkAlgNode::node_config_update(Config &config, uint32_t level)
00287 {
00288 this->alg_.lock();
00289
00290 ROS_INFO("Tool tcp frame: %s", config.tool_tcp.c_str());
00291 ROS_INFO("Robot tcp frame: %s", config.tool_tcp.c_str());
00292 tool_tcp_str_ = config.tool_tcp;
00293 robot_tcp_str_ = config.robot_tcp;
00294 if(!listener_.frameExists(config.tool_tcp))
00295 ROS_WARN("Frame %s does not exist, IK won't work until it is published",config.tool_tcp.c_str());
00296 if(!listener_.frameExists(config.robot_tcp))
00297 ROS_WARN("Frame %s does not exist, IK won't work until it is published",config.robot_tcp.c_str());
00298 this->alg_.unlock();
00299 }
00300
00301 void WamTcpIkAlgNode::addNodeDiagnostics(void)
00302 {
00303 }
00304
00305
00306 int main(int argc,char *argv[])
00307 {
00308 return algorithm_base::main<WamTcpIkAlgNode>(argc, argv, "wam_tcp_ik_alg_node");
00309 }