set_ref_frame.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::setRefFrame()
00042 {
00043   try
00044   {
00045     tf_listener_.waitForTransform(robot_base_str_, robot_ref_torso_str_, ros::Time(0), ros::Duration(wait_for_tf_));
00046     tf_listener_.lookupTransform(robot_base_str_, robot_ref_torso_str_, ros::Time(0), tf_robot_ref_torso_);
00047   }
00048   catch (tf::TransformException const &ex)
00049   {
00050     ROS_DEBUG("%s",ex.what());
00051     ROS_WARN("(Step 2) Couldn't get the transformation to the robot's torso.");
00052     ROS_WARN("No further processing will be done!");    
00053     return false;
00054   } 
00055 
00056   tf_ref_frame_.setOrigin(tf_robot_ref_torso_.getOrigin()); // use the position of the chosen robot _reference_ frame
00057   quat_ = tf::Quaternion(tf_usr_torso_.getRotation()); // and the rotation of the user torso
00058   // but (manually) rotate it to align with the convention (i.e. D-H) of the robot's frames
00059   quat_adjust_.setRPY(ref_frame_rot_vec_[0], ref_frame_rot_vec_[1], ref_frame_rot_vec_[2]);
00060   quat_ = quat_adjust_ * quat_;  
00061   tf_ref_frame_.setRotation(quat_);
00062   tf_broadcaster_.sendTransform(tf::StampedTransform(tf_ref_frame_, ros::Time::now(),
00063   robot_base_str_, "/ref_frame"));  
00064 
00065   // another reference frame without the operator's torso orientation, but with its convention
00066   quat_.setRPY(ref_frame_rot_vec_[0], ref_frame_rot_vec_[1], ref_frame_rot_vec_[2]);
00067   tf_ref_frame_.setRotation(quat_);
00068   tf_broadcaster_.sendTransform(tf::StampedTransform(tf_ref_frame_, ros::Time::now(),
00069   robot_base_str_, "/fixed_ref_frame")); 
00070 
00071   return true;
00072 }


motion_adaption
Author(s): Marcus Liebhardt
autogenerated on Mon Jan 6 2014 11:39:55