odom.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 "odom.hpp"
00022 #include "../tools/from_any_value.hpp"
00023 
00024 /*
00025 * BOOST includes
00026 */
00027 #include <boost/foreach.hpp>
00028 #define for_each BOOST_FOREACH
00029 
00030 
00031 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00032 
00033 namespace naoqi
00034 {
00035 namespace converter
00036 {
00037  
00038 OdomConverter::OdomConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ):
00039   BaseConverter( name, frequency, session ),
00040   p_motion_( session->service("ALMotion") )
00041   
00042 {
00043 }
00044 
00045 void OdomConverter::registerCallback( message_actions::MessageAction action, Callback_t cb )
00046 {
00047   callbacks_[action] = cb;
00048 }
00049 
00050 void OdomConverter::callAll( const std::vector<message_actions::MessageAction>& actions )
00051 {
00052   
00053   int FRAME_WORLD = 1;
00054   bool use_sensor = true;
00055   // documentation of getPosition available here: http://doc.aldebaran.com/2-1/naoqi/motion/control-cartesian.html
00056   std::vector<float> al_odometry_data = p_motion_.call<std::vector<float> >( "getPosition", "Torso", FRAME_WORLD, use_sensor );
00057   
00058   const ros::Time& odom_stamp = ros::Time::now();
00059   std::vector<float> al_speed_data = p_motion_.call<std::vector<float> >( "getRobotVelocity" );
00060   
00061   const float& odomX  =  al_odometry_data[0];
00062   const float& odomY  =  al_odometry_data[1];
00063   const float& odomZ  =  al_odometry_data[2];
00064   const float& odomWX =  al_odometry_data[3];
00065   const float& odomWY =  al_odometry_data[4];
00066   const float& odomWZ =  al_odometry_data[5];
00067   
00068   const float& dX = al_speed_data[0];
00069   const float& dY = al_speed_data[1];
00070   const float& dWZ = al_speed_data[2];
00071 
00072   //since all odometry is 6DOF we'll need a quaternion created from yaw
00073   tf2::Quaternion tf_quat;
00074   tf_quat.setRPY( odomWX, odomWY, odomWZ );
00075   geometry_msgs::Quaternion odom_quat = tf2::toMsg( tf_quat );
00076 
00077   static nav_msgs::Odometry msg_odom;
00078   msg_odom.header.frame_id = "odom";
00079   msg_odom.child_frame_id = "base_link";
00080   msg_odom.header.stamp = odom_stamp;
00081 
00082   msg_odom.pose.pose.orientation = odom_quat;
00083   msg_odom.pose.pose.position.x = odomX;
00084   msg_odom.pose.pose.position.y = odomY;
00085   msg_odom.pose.pose.position.z = odomZ;
00086   
00087   msg_odom.twist.twist.linear.x = dX;
00088   msg_odom.twist.twist.linear.y = dY;
00089   msg_odom.twist.twist.linear.z = 0;
00090   
00091   msg_odom.twist.twist.angular.x = 0;
00092   msg_odom.twist.twist.angular.y = 0;
00093   msg_odom.twist.twist.angular.z = dWZ;
00094 
00095   for_each( message_actions::MessageAction action, actions )
00096   {
00097     callbacks_[action](msg_odom);
00098     
00099   }
00100 }
00101 
00102 void OdomConverter::reset( )
00103 {
00104 }
00105 
00106 } //converter
00107 } // naoqi


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