moveto.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Aldebaran
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 /*
00019  * LOCAL includes
00020  */
00021 #include "moveto.hpp"
00022 
00023 /*
00024  * ROS includes
00025  */
00026 //#include <tf/transform_datatypes.h>
00027 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00028 
00029 #include "../helpers/transform_helpers.hpp"
00030 
00031 namespace naoqi
00032 {
00033 namespace subscriber
00034 {
00035 
00036 MovetoSubscriber::MovetoSubscriber( const std::string& name, const std::string& topic, const qi::SessionPtr& session,
00037                                     const boost::shared_ptr<tf2_ros::Buffer>& tf2_buffer):
00038   BaseSubscriber( name, topic, session ),
00039   p_motion_( session->service("ALMotion") ),
00040   tf2_buffer_( tf2_buffer )
00041 {}
00042 
00043 void MovetoSubscriber::reset( ros::NodeHandle& nh )
00044 {
00045   sub_moveto_ = nh.subscribe( topic_, 10, &MovetoSubscriber::callback, this );
00046   is_initialized_ = true;
00047 }
00048 
00049 void MovetoSubscriber::callback( const geometry_msgs::PoseStampedConstPtr& pose_msg )
00050 {
00051   if ( pose_msg->header.frame_id == "base_footprint" )
00052   {
00053     double yaw = helpers::transform::getYaw(pose_msg->pose);
00054 
00055     std::cout << "going to move x: " <<  pose_msg->pose.position.x << " y: " << pose_msg->pose.position.y << " z: " << pose_msg->pose.position.z << " yaw: " << yaw << std::endl;
00056     p_motion_.async<void>("moveTo", pose_msg->pose.position.x, pose_msg->pose.position.y, yaw);
00057   }
00058   else{
00059     geometry_msgs::PoseStamped pose_msg_bf;
00060     //geometry_msgs::TransformStamped tf_trans;
00061     //tf_listenerPtr_->waitForTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), ros::Duration(5) );
00062     bool canTransform = tf2_buffer_->canTransform("base_footprint", pose_msg->header.frame_id, ros::Time(0), ros::Duration(2) );
00063     if (!canTransform) {
00064       std::cout << "Cannot transform from " << pose_msg->header.frame_id << " to base_footprint" << std::endl;
00065       return;
00066     }
00067     try
00068     {
00069       //tf_listenerPtr_->lookupTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), tf_trans);
00070       //std::cout << "got a transform " << tf_trans.getOrigin().x() << std::endl;
00071       tf2_buffer_->transform( *pose_msg, pose_msg_bf, "base_footprint", ros::Time(0), pose_msg->header.frame_id );
00072       double yaw = helpers::transform::getYaw(pose_msg_bf.pose);
00073       std::cout << "odom to move x: " <<  pose_msg_bf.pose.position.x << " y: " << pose_msg_bf.pose.position.y << " z: " << pose_msg_bf.pose.position.z << " yaw: " << yaw << std::endl;
00074       p_motion_.async<void>("moveTo", pose_msg_bf.pose.position.x, pose_msg_bf.pose.position.y, yaw );
00075     } catch( const tf2::LookupException& e)
00076     {
00077       std::cout << e.what() << std::endl;
00078       std::cout << "moveto position in frame_id " << pose_msg->header.frame_id << "is not supported in any other base frame than basefootprint" << std::endl;
00079     }
00080     catch( const tf2::ExtrapolationException& e)
00081     {
00082       std::cout << "received an error on the time lookup" << std::endl;
00083     }
00084   }
00085 }
00086 
00087 } //publisher
00088 } // naoqi


naoqi_driver
Author(s): Karsten Knese
autogenerated on Sun Sep 17 2017 02:57:14