get_transforms.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 
00038 #include "motion_adaption/motion_adaption.h"
00039 
00040 
00041 bool MotionAdaption::getTransforms()
00042 {
00043   try
00044   {
00045     //torso
00046     tf_listener_.waitForTransform(world_ref_frame_str_, user_torso_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00047     tf_listener_.lookupTransform(world_ref_frame_str_, user_torso_str_, ros::Time(0), tf_usr_torso_);
00048     //head
00049     tf_listener_.waitForTransform(user_torso_str_, user_head_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00050     tf_listener_.lookupTransform(user_torso_str_, user_head_str_, ros::Time(0), tf_usr_head_);
00051     // right shoulder
00052     tf_listener_.waitForTransform(user_torso_str_, user_r_shoulder_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00053     tf_listener_.lookupTransform(user_torso_str_, user_r_shoulder_str_, ros::Time(0), tf_usr_r_shoulder_);
00054     // right shoulder to right elbow
00055     tf_listener_.waitForTransform(user_r_shoulder_str_, user_r_elbow_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00056     tf_listener_.lookupTransform(user_r_shoulder_str_, user_r_elbow_str_, ros::Time(0), tf_usr_r_shoulder_elbow_);
00057     // right elbow to right hand
00058     tf_listener_.waitForTransform(user_r_elbow_str_, user_r_hand_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00059     tf_listener_.lookupTransform(user_r_elbow_str_, user_r_hand_str_, ros::Time(0), tf_usr_r_elbow_hand_);
00060     // right elbow
00061     tf_listener_.waitForTransform(user_torso_str_, user_r_elbow_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00062     tf_listener_.lookupTransform(user_torso_str_, user_r_elbow_str_, ros::Time(0), tf_usr_r_elbow_);
00063     // right hand
00064     tf_listener_.waitForTransform(user_torso_str_, user_r_hand_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00065     tf_listener_.lookupTransform(user_torso_str_, user_r_hand_str_, ros::Time(0), tf_usr_r_hand_);
00066     // left shoulder
00067     tf_listener_.waitForTransform(user_torso_str_, user_l_shoulder_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00068     tf_listener_.lookupTransform(user_torso_str_, user_l_shoulder_str_, ros::Time(0), tf_usr_l_shoulder_);
00069     // left shoulder to left elbow
00070     tf_listener_.waitForTransform(user_l_shoulder_str_, user_l_elbow_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00071     tf_listener_.lookupTransform(user_l_shoulder_str_, user_l_elbow_str_, ros::Time(0), tf_usr_l_shoulder_elbow_);
00072     // left elbow to left hand
00073     tf_listener_.waitForTransform(user_l_elbow_str_, user_l_hand_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00074     tf_listener_.lookupTransform(user_l_elbow_str_, user_l_hand_str_, ros::Time(0), tf_usr_l_elbow_hand_);
00075     // left elbow
00076     tf_listener_.waitForTransform(user_torso_str_, user_l_elbow_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00077     tf_listener_.lookupTransform(user_torso_str_, user_l_elbow_str_, ros::Time(0), tf_usr_l_elbow_);
00078     // left hand
00079     tf_listener_.waitForTransform(user_torso_str_, user_l_hand_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00080     tf_listener_.lookupTransform(user_torso_str_, user_l_hand_str_, ros::Time(0), tf_usr_l_hand_);
00081   }
00082   catch (tf::TransformException const &ex)
00083   {
00084     ROS_DEBUG("%s",ex.what());
00085     ROS_WARN("(Step 1) Couldn't get one or more transformations of the user.");
00086     ROS_WARN("No further processing will be done!");
00087     return false;
00088   }   
00089   return true;
00090 }
00091 


motion_adaption
Author(s): Marcus Liebhardt
autogenerated on Mon Oct 6 2014 04:07:39