wviz_kinematic_manager: wviz_kinematic

00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2011, Robert Bosch LLC.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 
00037 #include <trajectory_msgs/JointTrajectory.h>
00038 #include <wviz_kinematic_manager/wviz_kinematic_manager.h>
00039 
00040 namespace wviz_kinematic_manager {
00041 
00042   KinematicService::KinematicService(ros::NodeHandle& node_)
00043   {
00044     ros::NodeHandle local_node("~");
00045     node = node_;
00046 
00047     kinematics_msgs::GetKinematicSolverInfo::Request request;
00048     kinematics_msgs::GetKinematicSolverInfo::Response response;
00049 
00050     // service call
00051     getpose_srv = local_node.advertiseService("/get_pose",&KinematicService::KinematicHandler, this);
00052     releasepose_srv = local_node.advertiseService("/release_pose",&KinematicService::KinematicRelease, this);
00053 
00054     // wait for service
00055     // right arm ik & fk solver
00056     ros::service::waitForService("pr2_right_arm_kinematics/get_ik_solver_info");
00057     ros::service::waitForService("pr2_right_arm_kinematics/get_ik");
00058     ros::service::waitForService("pr2_right_arm_kinematics/get_fk");
00059 
00060     r_query_client = node.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_right_arm_kinematics/get_ik_solver_info");
00061     r_ik_client = node.serviceClient<kinematics_msgs::GetPositionIK>("pr2_right_arm_kinematics/get_ik");                            
00062     r_fk_client = node.serviceClient<kinematics_msgs::GetPositionFK>("pr2_right_arm_kinematics/get_fk");                            
00063     
00064     
00065     // left ar ik & fk solver
00066     ros::service::waitForService("pr2_left_arm_kinematics/get_ik_solver_info");
00067     ros::service::waitForService("pr2_left_arm_kinematics/get_ik");
00068     ros::service::waitForService("pr2_left_arm_kinematics/get_fk");                
00069    
00070     l_query_client = node.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_left_arm_kinematics/get_ik_solver_info");
00071     l_ik_client = node.serviceClient<kinematics_msgs::GetPositionIK>("pr2_left_arm_kinematics/get_ik");
00072     l_fk_client = node.serviceClient<kinematics_msgs::GetPositionFK>("pr2_left_arm_kinematics/get_fk");
00073 
00074     pub_right = node.advertise<trajectory_msgs::JointTrajectory>("/r_arm_controller/command",5);
00075     pub_left = node.advertise<trajectory_msgs::JointTrajectory>("/l_arm_controller/command",5);
00076     publishReady = false;
00077 
00078 
00079     // get right arm's solver info
00080     if(r_query_client.call(request,response))
00081     {
00082       r_solver_info = response.kinematic_solver_info;
00083       for(unsigned int i = 0 ; i < response.kinematic_solver_info.joint_names.size(); i++)
00084       {
00085         ROS_INFO("Joint: %d %s",i,r_solver_info.joint_names[i].c_str());
00086       }
00087     }
00088     else 
00089     {
00090       ROS_ERROR("Could not call query service");
00091       ros::shutdown();
00092       exit(1);
00093     }
00094 
00095     // get left arm's solver info
00096     if(l_query_client.call(request,response))
00097     {
00098       l_solver_info = response.kinematic_solver_info;
00099       for(unsigned int i = 0 ; i < response.kinematic_solver_info.joint_names.size(); i++)
00100       {
00101         ROS_INFO("Joint: %d %s",i,l_solver_info.joint_names[i].c_str());
00102       }
00103     }
00104     else 
00105     {
00106       ROS_ERROR("Could not call query service");
00107       ros::shutdown();
00108       exit(1);
00109     }
00110   }
00111 
00112   KinematicService::~KinematicService()
00113   {
00114   }
00115 
00116   bool KinematicService::KinematicHandler(wviz_kinematic_manager::getPose::Request& req, wviz_kinematic_manager::getPose::Response& resp)
00117   {
00118 /*    ROS_INFO("Target header link = %s",req.target_header_link.c_str());
00119     ROS_INFO("Target Links       = %s",req.target_links[0].c_str());                                                                                                                
00120     ROS_INFO("Position      : %6.3f %6.3f %6.3f",req.target_pose.position.x,req.target_pose.position.y,req.target_pose.position.z);                                                 
00121     ROS_INFO("Orientation   : %6.3f %6.3f %6.3f %6.3f",req.target_pose.orientation.x,req.target_pose.orientation.y,req.target_pose.orientation.z,req.target_pose.orientation.w);    
00122   */  
00123     // remove '/'
00124     for(unsigned int i = 0; i < req.target_links.size(); i++) {
00125       if(req.target_links[i][0] == '/')
00126       {
00127         req.target_links[i] = req.target_links[i].substr(1,req.target_links[i].length()-1);
00128 //        std::cout<< req.target_links[i] << std::endl;    
00129       }
00130     }
00131      
00132     kinematics_msgs::GetPositionIK::Response IKSolution = getIKSol(req.target_links[0],req.target_links[req.target_links.size()-1],req.target_pose); 
00133 
00134     if(IKSolution.error_code.val != IKSolution.error_code.SUCCESS)
00135     {
00136       ROS_INFO("IK Error Code = %d",IKSolution.error_code.val);
00137       resp.error_code = IKSolution.error_code.val;
00138       return true;
00139     }
00140     else {
00141       lastIKSol = IKSolution;
00142       lastSol = IKSolution.solution.joint_state;
00143     }
00144 
00145     kinematics_msgs::GetPositionFK::Response FKSolution = getFKSol(req.target_links,IKSolution.solution.joint_state);
00146 
00147     if(FKSolution.error_code.val != FKSolution.error_code.SUCCESS)
00148     {
00149       ROS_INFO("FK Error Code = %d",FKSolution.error_code.val);
00150       return true;                                                    
00151     }
00152     else {
00153       lastFKSol = FKSolution;
00154     }
00155 
00156     resp.error_code = FKSolution.error_code.val;
00157     resp.pose_stamped = FKSolution.pose_stamped;
00158     resp.link_names = FKSolution.fk_link_names;
00159     ROS_INFO("Success!");
00160     moveArm(lastSol);
00161     publishReady = true;
00162 
00163     return true;
00164   }
00165 
00166   kinematics_msgs::GetPositionIK::Response KinematicService::getIKSol(std::string target,std::string target_header_link,geometry_msgs::Pose& target_pose)
00167   {
00168     kinematics_msgs::GetPositionIK::Request request;
00169     kinematics_msgs::GetPositionIK::Response response;
00170     ros::ServiceClient ik_client;
00171     kinematics_msgs::KinematicSolverInfo solver_info;
00172 
00173     if(target[0] == 'r')
00174     {
00175       solver_info = r_solver_info;
00176       ik_client = r_ik_client;
00177       request.ik_request.ik_link_name = "r_wrist_roll_link";
00178     }
00179     else {
00180       solver_info = l_solver_info;
00181       ik_client = l_ik_client;
00182       request.ik_request.ik_link_name = "l_wrist_roll_link";
00183     }
00184 
00185     request.timeout = ros::Duration(5.0);
00186 //    request.ik_request.ik_link_name = target;
00187     
00188     request.ik_request.pose_stamped.header.frame_id = target_header_link;
00189     request.ik_request.pose_stamped.pose = target_pose;
00190 
00191     request.ik_request.ik_seed_state.joint_state.position.resize(solver_info.joint_names.size());
00192     request.ik_request.ik_seed_state.joint_state.name = solver_info.joint_names;
00193     
00194     // inverse kinematic service call
00195     if(ik_client.call(request,response))
00196     {
00197       return response;
00198     }
00199     else {
00200       ROS_ERROR("Inverse kinematics service call failed");
00201       return response;
00202     }
00203   }
00204   
00205   kinematics_msgs::GetPositionFK::Response KinematicService::getFKSol(std::vector<std::string> target_links,sensor_msgs::JointState joint_state)
00206   {
00207     kinematics_msgs::GetPositionFK::Request request; 
00208     kinematics_msgs::GetPositionFK::Response response; 
00209     kinematics_msgs::GetPositionFK::Response result; 
00210     ros::ServiceClient fk_client;
00211     
00212     if((target_links[0])[0] =='r')
00213       fk_client = r_fk_client;
00214     else
00215       fk_client = l_fk_client;
00216 
00217     request.robot_state.joint_state = joint_state;
00218     for(unsigned int i = target_links.size()-1; i > 0; i --) {
00219       request.header.frame_id = target_links[i];
00220       request.fk_link_names.clear();
00221       request.fk_link_names.push_back(target_links[i-1]);
00222   
00223       if(fk_client.call(request,response))
00224       {
00225         if(response.error_code.val == response.error_code.SUCCESS) {
00226           result.fk_link_names.push_back(response.fk_link_names[0]);
00227           result.pose_stamped.push_back(response.pose_stamped[0]);
00228           result.error_code = response.error_code;
00229         }
00230         else {
00231           result.error_code = response.error_code;
00232           break;
00233         }
00234       }
00235       else {
00236         ROS_ERROR("Forward Kinematis service call failed");
00237         result.error_code = response.error_code;
00238         break;
00239       }
00240     }
00241 
00242     return result;
00243   }
00244 
00245   bool KinematicService::KinematicRelease(wviz_kinematic_manager::releasePose::Request& req, wviz_kinematic_manager::releasePose::Response& resp)
00246   {
00247     if(req.choice == 1)
00248     {
00249       moveArm(lastSol);
00250     }
00251     else if(req.choice == 2)
00252     {
00253       sensor_msgs::JointState js;
00254       js.name.push_back("l_shoulder_pan_joint");
00255       js.name.push_back("l_shoulder_lift_joint");
00256       js.name.push_back("l_upper_arm_roll_joint");
00257       js.name.push_back("l_elbow_flex_joint");
00258       js.name.push_back("l_forearm_roll_joint");
00259       js.name.push_back("l_wrist_flex_joint");
00260       js.name.push_back("l_wrist_roll_joint");
00261       js.position.push_back(0.0);
00262       js.position.push_back(0.0);
00263       js.position.push_back(0.0);
00264       js.position.push_back(0.0);
00265       js.position.push_back(0.0);
00266       js.position.push_back(0.0);
00267       js.position.push_back(0.0);
00268       moveArm(js);
00269       js.name.clear();
00270       js.name.push_back("r_shoulder_pan_joint");       
00271       js.name.push_back("r_shoulder_lift_joint");
00272       js.name.push_back("r_upper_arm_roll_joint");
00273       js.name.push_back("r_elbow_flex_joint");
00274       js.name.push_back("r_forearm_roll_joint");
00275       js.name.push_back("r_wrist_flex_joint");
00276       js.name.push_back("r_wrist_roll_joint");
00277       moveArm(js);
00278       lastSol = js;
00279     }
00280     return true;
00281   }
00282 
00283   void KinematicService::moveArm(sensor_msgs::JointState joint_state)
00284   {
00285     trajectory_msgs::JointTrajectory jt;
00286     trajectory_msgs::JointTrajectoryPoint jtp;
00287     jtp.velocities.push_back(0.0);
00288     jtp.velocities.push_back(0.0);
00289     jtp.velocities.push_back(0.0);
00290     jtp.velocities.push_back(0.0);
00291     jtp.velocities.push_back(0.0);
00292     jtp.velocities.push_back(0.0);
00293     jtp.velocities.push_back(0.0);
00294 
00295     jt.joint_names = joint_state.name;
00296     jtp.positions = joint_state.position;
00297     jt.points.push_back(jtp);
00298 
00299     if(jt.joint_names[0][0] == 'r') {
00300       pub_right.publish(jt);
00301     }
00302     else {
00303       pub_left.publish(jt);
00304     }
00305   }
00306 
00307 
00308   void KinematicService::spin()
00309   {
00310     ros::Rate r(10);
00311     
00312     while(node.ok()) {
00313       if(publishReady == true) {
00314 //        moveArm();
00315         publishReady = false;
00316       }
00317       ros::spinOnce();
00318       r.sleep();
00319     }
00320 
00321   }
00322 }
00323 
00324 int main(int argc, char** argv)
00325 {
00326   ros::init(argc,argv,"wviz_kinematic_manager");
00327 
00328   ros::NodeHandle nh;
00329   wviz_kinematic_manager::KinematicService manager(nh);
00330   ROS_INFO("Node Initialized");
00331   manager.spin();
00332 
00333 
00334   return 0;
00335 }
wviz_kinematic_manager.cpp
