reem_teleop_coordinator.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Modified BSD License)
00003  *
00004  *  Copyright (c) 2011, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of PAL Robotics, S.L. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00047 #include <string>
00048 #include <map>
00049 #include <vector>
00050 
00051 #include <geometry_msgs/PoseStamped.h>
00052 #include <geometry_msgs/PoseArray.h>
00053 #include <geometry_msgs/TransformStamped.h>
00054 #include <kinematics_msgs/GetPositionFK.h>
00055 #include <ros/ros.h>
00056 #include <sensor_msgs/JointState.h>
00057 #include <tf/transform_listener.h>
00058 #include <tf/transform_broadcaster.h>
00059 #include <tree_kinematics/get_tree_position_ik.h>
00060 #include <arm_navigation_msgs/GetPlanningScene.h>
00061 #include <planning_environment/models/collision_models.h>
00062 
00063 #include <trajectory_msgs/JointTrajectory.h>
00064 
00065 
00066 
00067 
00068 
00069 static const std::string FK_SERVICE = "/tree_kinematics_node/get_position_fk";
00070 static const std::string IK_SERVICE = "/tree_kinematics_node/get_position_ik";
00071 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
00072 static const std::string PUB_TOPIC_JOINT_STATES_CMD = "/joint_states_cmd";
00073 static const std::string SUB_TOPIC_JOINT_STATES = "/joint_states";
00074 
00075 bool joint_states_valid = false;
00076 sensor_msgs::JointState old_joint_state;
00077 
00078 // pointer to the current joint states
00079 sensor_msgs::JointState::ConstPtr joint_states_ptr;
00080 
00091 void jointStatesCB(const sensor_msgs::JointState::ConstPtr& joint_states)
00092 {
00093   joint_states_ptr = joint_states;
00094   if(!joint_states_valid)
00095     old_joint_state = *joint_states;
00096   joint_states_valid = true;
00097 }
00098 
00099 
00113 bool getGoalTransform(tf::TransformListener& tf_listener,
00114                       std::string& root_frame_name,
00115                       std::string goal_frame_name,
00116                       geometry_msgs::PoseStamped& pose)
00117 {
00118   tf::StampedTransform transform;
00119   try
00120   {
00121     tf_listener.waitForTransform(root_frame_name, goal_frame_name, ros::Time(0), ros::Duration(0.5));
00122     tf_listener.lookupTransform(root_frame_name, goal_frame_name, ros::Time(0), transform);
00123   }
00124   catch (tf::TransformException const &ex)
00125   {
00126     ROS_DEBUG("%s",ex.what());
00127     ROS_WARN("No transformation available from '%s' to '%s'", root_frame_name.c_str(), goal_frame_name.c_str());
00128     return false;
00129   }
00130   pose.header.stamp = ros::Time::now();
00131   pose.header.frame_id = root_frame_name;
00132   pose.pose.position.x = transform.getOrigin().x();
00133   pose.pose.position.y = transform.getOrigin().y();
00134   pose.pose.position.z = transform.getOrigin().z();
00135   pose.pose.orientation.x = transform.getRotation().x();
00136   pose.pose.orientation.y = transform.getRotation().y();
00137   pose.pose.orientation.z = transform.getRotation().z();
00138   pose.pose.orientation.w = transform.getRotation().w();
00139 
00140   return true;  
00141 }
00142 
00143 
00144 int main(int argc, char** argv)
00145 {
00146   using namespace arm_navigation_msgs;
00147 
00148   ros::init(argc, argv, "reem_teleop_coordinator_node");
00149   ros::NodeHandle nh, nh_private("~");
00150 
00151   int nr_of_endpts = 0;
00152   std::vector<std::string> endpts;
00153   endpts.clear();
00154   if(nh_private.getParam("nr_of_endpoints", nr_of_endpts) && nr_of_endpts >= 0)
00155   {
00156     for(int i = 0; i < nr_of_endpts; ++i)
00157     {
00158       std::string elementname;
00159       std::string endpt_name;    
00160       std::stringstream ss;
00161       ss << "endpoint_" << i;
00162       ss >> elementname;
00163       if(nh_private.getParam(elementname, endpt_name))  
00164       {
00165         endpts.push_back(endpt_name);
00166         ROS_INFO("Added endpoint '%s'", endpts[i].c_str());
00167       }
00168       else
00169       {
00170         ROS_FATAL("Couldn't get the name of an endpoint! Aborting...");
00171         return 0;
00172       }
00173     }
00174   }
00175   
00176   std::vector<std::string> comand_names;
00177 
00178   geometry_msgs::PoseStamped pose;
00179   std::map<std::string, geometry_msgs::PoseStamped> poses;
00180   std::map<std::string, std::string> rel_endpoints_goals; 
00181   for(unsigned int i = 0; i < endpts.size(); ++i)
00182   {
00183     std::string elementname;
00184     std::string endpt_goal_name;    
00185     std::stringstream ss;
00186     ss << "endpoint_" << i << "_goal";
00187     ss >> elementname;
00188     if(nh_private.getParam(elementname, endpt_goal_name))  
00189     {
00190       poses.insert(std::map<std::string, geometry_msgs::PoseStamped>::value_type(endpt_goal_name, pose));
00191       rel_endpoints_goals.insert(std::map<std::string, std::string>::value_type(endpt_goal_name, endpts[i])); 
00192       ROS_INFO("Added endpoint goal '%s'", endpt_goal_name.c_str());
00193       ROS_INFO("Goal '%s' is linked to endpoint '%s'", endpt_goal_name.c_str(), endpts[i].c_str());
00194     }
00195     else
00196     {
00197       ROS_FATAL("Couldn't get the name of an endpoint goal! Aborting...");
00198       return 0;
00199     }  
00200   }
00201   
00202   std::string root_frame_name;
00203   if(nh_private.getParam("root_frame_name", root_frame_name))  
00204     ROS_INFO("root frame name: %s", root_frame_name.c_str());
00205   else
00206   {
00207     ROS_FATAL("Couldn't get the name of the root frame! Aborting...");
00208     return 0;
00209   }
00210   
00211   tf::TransformListener tf_listener;
00212   tf::TransformBroadcaster tf_broadcaster;
00213   geometry_msgs::TransformStamped transform;
00214 
00215   // subscriber for current joint states
00216   ros::Subscriber sub_joint_states = nh.subscribe(SUB_TOPIC_JOINT_STATES, 20, jointStatesCB); 
00217   
00218   // service client for IK calculations
00219   tree_kinematics::get_tree_position_ik tree_ik_srv;
00220   //ros::ServiceClient tree_ik_srv_client = nh.serviceClient<tree_kinematics::get_tree_position_ik>
00221   //(IK_SERVICE, true);
00222 
00223   // service client for FK calculations
00224   kinematics_msgs::GetPositionFK tree_fk_srv;
00225   //ros::ServiceClient tree_fk_srv_client = nh.serviceClient<kinematics_msgs::GetPositionFK>
00226   //(FK_SERVICE, true);
00227   
00228   // service client for self-collision and joint limits checking
00229   bool no_self_collision = false;
00230   bool check_self_collision, check_joint_limits = true;
00231   nh_private.param("check_self_collision", check_self_collision, true);
00232   nh_private.param("check_joint_limits", check_joint_limits, true);
00233   ROS_INFO("checking for self-collision: %s, checking for joint limits: %s", (check_self_collision)?"true":"false",
00234   (check_joint_limits)?"true":"false");
00235   
00236   // publisher for joint states commands
00237   ros::Publisher pub_joint_states_cmd = nh.advertise<sensor_msgs::JointState>(PUB_TOPIC_JOINT_STATES_CMD, 1);
00238 
00239   ros::Publisher traj_publisher_ = nh.advertise<trajectory_msgs::JointTrajectory>("/upper_body_controller/command", 1);
00240 
00241   sensor_msgs::JointState joint_states_cmd;
00242   /*
00243   sensor_msgs::JointState old_joint_state;
00244   old_joint_state.name = std::vector<std::string>(24, "empty");
00245   old_joint_state.position = std::vector<double>(24, 0.0);
00246   old_joint_state.velocity = std::vector<double>(24, 0.0);
00247   old_joint_state.effort = std::vector<double>(24, 0.0);    
00248   */
00249   
00250   int loop_rate_value;
00251   nh_private.param("loop_rate", loop_rate_value, 10);
00252   ROS_INFO("loop rate: %d", loop_rate_value);
00253   ros::Rate loop_rate(loop_rate_value);
00254   unsigned int loop_count = 1;
00255   double cycle_time_median = 0.0;
00256   double ik_duration = 0.0;
00257   double ik_duration_median = 0.0;
00258   double scc_duration = 0.0;
00259   double scc_duration_median = 0.0;
00260   double cjp_duration = 0.0;
00261   double cjp_duration_median = 0.0;
00262   
00263   ros::service::waitForService(IK_SERVICE);
00264   ros::ServiceClient tree_ik_srv_client = nh.serviceClient<tree_kinematics::get_tree_position_ik> (IK_SERVICE, true);
00265   
00266   ros::service::waitForService(FK_SERVICE);
00267   ros::ServiceClient tree_fk_srv_client = nh.serviceClient<kinematics_msgs::GetPositionFK> (FK_SERVICE, true);
00268   
00269   ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
00270   ros::ServiceClient get_planning_scene_client = nh.serviceClient<GetPlanningScene>(SET_PLANNING_SCENE_DIFF_NAME, true);
00271   planning_environment::CollisionModels collision_models("robot_description");
00272   collision_models.disableCollisionsForNonUpdatedLinks("upper_body");
00273   std::vector<std::string> joint_names = collision_models.getKinematicModel()->getModelGroup("upper_body")->getJointModelNames();
00274   while (nh.ok())
00275   {
00276     ros::spinOnce();
00277 
00278     // transform goal frames into pose messages      
00279     std::map<std::string, geometry_msgs::PoseStamped>::iterator poses_it;
00280     geometry_msgs::PoseStamped pose;
00281     bool goal_transforms_valid = true;
00282     for(poses_it = poses.begin(); poses_it != poses.end(); ++poses_it)
00283     {
00284       if(getGoalTransform(tf_listener, root_frame_name, poses_it->first, pose))
00285         poses_it->second = pose;
00286       else
00287       {
00288         ROS_WARN_THROTTLE(0.5, "No valid transformation available for endpoint goal '%s'", poses_it->first.c_str());
00289         ROS_WARN_THROTTLE(0.5, "No commands will be calculated.");
00290         goal_transforms_valid = false;
00291       }
00292     }
00293     
00294     if (joint_states_valid && goal_transforms_valid)
00295     {
00296       // IK calculations
00297 
00298       // feed pose messages for every endpoint into the request
00299       tree_ik_srv.request.pos_ik_request.clear();
00300       kinematics_msgs::PositionIKRequest pos_ik_request;
00301       std::map<std::string, std::string>::iterator rel_it;
00302       for(poses_it = poses.begin(); poses_it != poses.end(); ++poses_it)
00303       {
00304         rel_it = rel_endpoints_goals.find(poses_it->first);
00305         pos_ik_request.ik_link_name = rel_it->second;
00306         pos_ik_request.pose_stamped = poses_it->second;
00307         tree_ik_srv.request.pos_ik_request.push_back(pos_ik_request);
00308       }
00309 
00310       // feeding current joint positions into the request
00311       if (joint_states_ptr)
00312         //tree_ik_srv.request.pos_ik_request[0].ik_seed_state.joint_state = *joint_states_ptr;
00313         tree_ik_srv.request.pos_ik_request[0].ik_seed_state.joint_state = old_joint_state;
00314       else
00315       {
00316         ROS_ERROR("joint_states_ptr invalid! Aborting loop ...");
00317         continue;
00318       }  
00319       /*
00320       ROS_DEBUG("Following information has been gathered:");
00321       for(unsigned int i = 0; i < tree_ik_srv.request.pos_ik_request.size(); ++i)
00322       {
00323         if( i == 0 )
00324         {
00325           for(unsigned int j = 0;
00326                            j < tree_ik_srv.request.pos_ik_request[i].ik_seed_state.joint_state.name.size();
00327                            ++j)
00328             ROS_DEBUG("joint[%d]: %s with position %f",
00329                       j,
00330                       tree_ik_srv.request.pos_ik_request[i].ik_seed_state.joint_state.name[j].c_str(),
00331                       tree_ik_srv.request.pos_ik_request[i].ik_seed_state.joint_state.position[j]);   
00332         }          
00333         ROS_DEBUG("link[%d]: %s with x = %f, y = %f, z = %f", i,
00334         tree_ik_srv.request.pos_ik_request[i].ik_link_name.c_str(),
00335         tree_ik_srv.request.pos_ik_request[i].pose_stamped.pose.position.x,
00336         tree_ik_srv.request.pos_ik_request[i].pose_stamped.pose.position.y,
00337         tree_ik_srv.request.pos_ik_request[i].pose_stamped.pose.position.z);
00338       }
00339       */
00340       
00341       ik_duration = ros::Time::now().toSec();
00342       if (tree_ik_srv_client.call(tree_ik_srv))
00343       {
00344         for(unsigned int i = 0; i < tree_ik_srv.response.solution.joint_state.name.size(); ++i)
00345         {
00346           ROS_DEBUG_THROTTLE(1.0, "desired position for joint[%d]('%s'): %f", i,
00347           tree_ik_srv.response.solution.joint_state.name[i].c_str(), 
00348           tree_ik_srv.response.solution.joint_state.position[i]);
00349         }
00350       }
00351       else
00352       {
00353         ROS_ERROR_THROTTLE(1.0, "get_tree_position_ik service call failed! Aborting loop ...");
00354         continue;
00355       }
00356       ik_duration = ros::Time::now().toSec() - ik_duration;
00357       ik_duration_median = ((ik_duration_median * (loop_count - 1)) + ik_duration) / loop_count;
00358       
00359       // self-collision checking
00360       GetPlanningScene::Request planning_scene_req;
00361       GetPlanningScene::Response planning_scene_res;
00362       if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res))
00363       {
00364         ROS_WARN("Can't get planning scene");
00365         continue;
00366       }
00367       planning_models::KinematicState* state = collision_models.setPlanningScene(planning_scene_res.planning_scene);
00368       
00369       std::map<std::string, double> nvalues;
00370       const sensor_msgs::JointState& sol_state = tree_ik_srv.response.solution.joint_state;
00371       for (size_t ith_joint = 0; ith_joint < sol_state.name.size(); ++ith_joint)
00372       {
00373         nvalues[sol_state.name[ith_joint]] = sol_state.position[ith_joint];
00374       }
00375       state->setKinematicState(nvalues);
00376       if (check_joint_limits && !state->areJointsWithinBounds(joint_names))
00377       {
00378         ROS_WARN_THROTTLE(1.0, "Requested state violates joint limits.");
00379       }
00380       else if (check_self_collision && collision_models.isKinematicStateInCollision(*state))
00381       {
00382         ROS_WARN_THROTTLE(1.0, "Requested state is in collision.");
00383       }
00384       else
00385       {
00386         ROS_DEBUG_THROTTLE(1.0, "Requested state is valid.");
00387         no_self_collision = true;
00388       }
00389       collision_models.revertPlanningScene(state);
00390 
00391       scc_duration = ros::Time::now().toSec();
00392       scc_duration = ros::Time::now().toSec() - scc_duration;
00393       scc_duration_median = ((scc_duration_median * (loop_count - 1)) + scc_duration) / loop_count;
00394       
00395       // commanding joint positions
00396       //no_self_collision = true;
00397       if(no_self_collision == true)
00398       {
00399         joint_states_cmd = tree_ik_srv.response.solution.joint_state;
00400         joint_states_cmd.header.stamp = ros::Time::now();
00401         pub_joint_states_cmd.publish(joint_states_cmd);
00402        
00403         trajectory_msgs::JointTrajectory   traj_;
00404         traj_.header.stamp = ros::Time::now();
00405 
00406         traj_.joint_names = joint_states_cmd.name;
00407         traj_.points.resize(1);
00408         traj_.points[0].positions = joint_states_cmd.position;
00409         traj_.points[0].velocities = joint_states_cmd.velocity;
00410         traj_.points[0].time_from_start = ros::Duration(0.001);
00411         traj_publisher_.publish(traj_);
00412 
00413         old_joint_state = joint_states_cmd;
00414         no_self_collision = false;       
00415 
00416       }
00417       else
00418       {
00419         for(unsigned int i = 0; i < old_joint_state.name.size(); ++i)
00420           old_joint_state.velocity[i] = 0.0;
00421         joint_states_cmd = old_joint_state;
00422         joint_states_cmd.header.stamp = ros::Time::now();
00423         pub_joint_states_cmd.publish(joint_states_cmd);
00424 
00425         trajectory_msgs::JointTrajectory   traj_;
00426         traj_.header.stamp = ros::Time::now();
00427         traj_.points.resize(1);
00428 
00429         for(int k=0; k<joint_states_cmd.name.size(); ++k){
00430 
00431           if(std::find(comand_names.begin(), comand_names.end(),joint_states_cmd.name[k] ) != comand_names.end()){
00432             traj_.joint_names.push_back(joint_states_cmd.name[k]);
00433             traj_.points[0].positions.push_back(joint_states_cmd.position[k]);
00434             traj_.points[0].velocities.push_back(joint_states_cmd.velocity[k]);
00435           }
00436         }
00437         traj_.points[0].time_from_start = ros::Duration(0.001);
00438         traj_publisher_.publish(traj_);
00439         
00440       }
00441       
00442       /* TODO, ARE THIS TF NEEDED ANY MORE?
00443       // For debug purpose: publish forward kinematics messages on tf
00444       cjp_duration = ros::Time::now().toSec();
00445        
00446       tree_fk_srv.request.header.stamp = ros::Time::now();
00447       tree_fk_srv.request.header.frame_id = "/base_footprint";      
00448       tree_fk_srv.request.robot_state.joint_state = tree_ik_srv.response.solution.joint_state;
00449       tree_fk_srv.request.robot_state.joint_state = *joint_states_ptr;                  
00450       tree_fk_srv.request.fk_link_names.resize(8);
00451       tree_fk_srv.request.fk_link_names[0] = "arm_right_2_link";
00452       tree_fk_srv.request.fk_link_names[1] = "arm_right_4_link";
00453       tree_fk_srv.request.fk_link_names[2] = "hand_right_link";
00454       tree_fk_srv.request.fk_link_names[3] = "arm_left_2_link";      
00455       tree_fk_srv.request.fk_link_names[4] = "arm_left_4_link";      
00456       tree_fk_srv.request.fk_link_names[5] = "hand_left_link";
00457       tree_fk_srv.request.fk_link_names[6] = "head_2_link";
00458       tree_fk_srv.request.fk_link_names[7] = "torso_2_link";      
00459       if (tree_fk_srv_client.call(tree_fk_srv))
00460       {
00461         if(tree_fk_srv.request.fk_link_names.size() == tree_fk_srv.response.pose_stamped.size())
00462         {
00463           for(unsigned int i = 0; i < tree_fk_srv.response.pose_stamped.size(); ++i)
00464           {
00465             std::stringstream ss;
00466             std::string name;
00467             ss << "fk_" << tree_fk_srv.request.fk_link_names[i];
00468             ss >> name;
00469             transform.header.seq++;
00470             transform.header.stamp = ros::Time::now();
00471             transform.header.frame_id = tree_fk_srv.request.header.frame_id;
00472             transform.child_frame_id = name;   
00473             transform.transform.translation.x = tree_fk_srv.response.pose_stamped[i].pose.position.x;
00474             transform.transform.translation.y = tree_fk_srv.response.pose_stamped[i].pose.position.y;
00475             transform.transform.translation.z = tree_fk_srv.response.pose_stamped[i].pose.position.z;                    
00476             transform.transform.rotation = tree_fk_srv.response.pose_stamped[i].pose.orientation;          
00477             tf_broadcaster.sendTransform(transform);
00478           }
00479         }
00480       }
00481       else
00482       {
00483         ROS_ERROR("get_tree_position_fk service call failed! Aborting loop ...");
00484         continue;
00485       }
00486         
00487       cjp_duration = ros::Time::now().toSec() - cjp_duration;
00488       cjp_duration_median = ((cjp_duration_median * (loop_count - 1)) + cjp_duration) / loop_count; 
00489         */
00490     }
00491     
00492     ros::spinOnce();
00493 
00494     loop_rate.sleep();
00495     
00496     cycle_time_median = ((cycle_time_median * (loop_count - 1)) + loop_rate.cycleTime().toSec()) / loop_count;
00497 
00498     ROS_DEBUG_THROTTLE(1.0, "reem_teleop: cycle time %f and median cycle time %f",
00499     loop_rate.cycleTime().toSec(), cycle_time_median);
00500     ROS_DEBUG_THROTTLE(1.0, "reem_teleop: IKC current duration %f and median %f", ik_duration, ik_duration_median);
00501     ROS_DEBUG_THROTTLE(1.0, "reem_teleop: SCC current duration %f and median %f", scc_duration, scc_duration_median);
00502     ROS_DEBUG_THROTTLE(1.0, "reem_teleop: FKC current duration %f and median %f", cjp_duration, cjp_duration_median);
00503 
00504     loop_count ++;
00505   }
00506   tree_ik_srv_client.shutdown();
00507   get_planning_scene_client.shutdown();
00508   pub_joint_states_cmd.shutdown();
00509   tree_fk_srv_client.shutdown();
00510 
00511   return 0;
00512 }
00513 


reem_teleop_coordinator
Author(s): Marcus Liebhardt
autogenerated on Mon Jan 6 2014 11:40:12