imu.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 "imu.hpp"
00022 #include "../tools/from_any_value.hpp"
00023 
00024 /*
00025 * ROS includes
00026 */
00027 //#include <tf/transform_datatypes.h>
00028 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00029 
00030 /*
00031 * BOOST includes
00032 */
00033 #include <boost/foreach.hpp>
00034 #define for_each BOOST_FOREACH
00035 
00036 namespace naoqi
00037 {
00038 namespace converter {
00039 
00040 
00041   ImuConverter::ImuConverter(const std::string& name, const IMU::Location& location,  const float& frequency, const qi::SessionPtr& session):
00042     BaseConverter(name, frequency, session),
00043     p_memory_(session->service("ALMemory"))
00044   {
00045     if(location == IMU::TORSO){
00046       msg_imu_.header.frame_id = "base_link";
00047       data_names_list_.push_back("DCM/Time");
00048       data_names_list_.push_back("Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value");
00049       data_names_list_.push_back("Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value");
00050       data_names_list_.push_back("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value");
00051       data_names_list_.push_back("Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value");
00052       data_names_list_.push_back("Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value");
00053       data_names_list_.push_back("Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value");
00054       data_names_list_.push_back("Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value");
00055       data_names_list_.push_back("Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value");
00056       data_names_list_.push_back("Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value");
00057     }
00058     else if(location == IMU::BASE){
00059       msg_imu_.header.frame_id = "base_footprint";
00060       data_names_list_.push_back("DCM/Time");
00061       data_names_list_.push_back("Device/SubDeviceList/InertialSensorBase/AngleX/Sensor/Value");
00062       data_names_list_.push_back("Device/SubDeviceList/InertialSensorBase/AngleY/Sensor/Value");
00063       data_names_list_.push_back("Device/SubDeviceList/InertialSensorBase/AngleZ/Sensor/Value");
00064       data_names_list_.push_back("Device/SubDeviceList/InertialSensorBase/GyroscopeX/Sensor/Value");
00065       data_names_list_.push_back("Device/SubDeviceList/InertialSensorBase/GyroscopeY/Sensor/Value");
00066       data_names_list_.push_back("Device/SubDeviceList/InertialSensorBase/GyroscopeZ/Sensor/Value");
00067       data_names_list_.push_back("Device/SubDeviceList/InertialSensorBase/AccelerometerX/Sensor/Value");
00068       data_names_list_.push_back("Device/SubDeviceList/InertialSensorBase/AccelerometerY/Sensor/Value");
00069       data_names_list_.push_back("Device/SubDeviceList/InertialSensorBase/AccelerometerZ/Sensor/Value");
00070     }
00071   }
00072 
00073   ImuConverter::~ImuConverter()
00074   {
00075 
00076   }
00077 
00078   void ImuConverter::reset()
00079   {
00080 
00081   }
00082 
00083   void ImuConverter::registerCallback( const message_actions::MessageAction action, Callback_t cb )
00084   {
00085     callbacks_[action] = cb;
00086   }
00087 
00088   void ImuConverter::callAll(const std::vector<message_actions::MessageAction>& actions)
00089   {
00090     // Get inertial data
00091     std::vector<float> memData;
00092     try {
00093         qi::AnyValue anyvalues = p_memory_.call<qi::AnyValue>("getListData", data_names_list_);
00094         tools::fromAnyValueToFloatVector(anyvalues, memData);
00095     } catch (const std::exception& e) {
00096       std::cerr << "Exception caught in ImuConverter: " << e.what() << std::endl;
00097       return;
00098     }
00099     // angle (X,Y,Z) = memData(1,2,3);
00100     // gyro  (X,Y,Z) = memData(4,5,6);
00101     // acc   (X,Y,Z) = memData(7,8,9);
00102 
00103     const ros::Time& stamp = ros::Time::now();
00104     msg_imu_.header.stamp = stamp;
00105 
00106     tf2::Quaternion tf_quat;
00107     tf_quat.setRPY( memData[1], memData[2], memData[3] );
00108     msg_imu_.orientation = tf2::toMsg( tf_quat );
00109 
00110     //msg_imu_.orientation = tf::createQuaternionMsgFromRollPitchYaw(
00111     //                            memData[1],
00112     //                            memData[2],
00113     //                            memData[3]);
00114 
00115     msg_imu_.angular_velocity.x = memData[4];
00116     msg_imu_.angular_velocity.y = memData[5];
00117     msg_imu_.angular_velocity.z = memData[6];
00118 
00119     msg_imu_.linear_acceleration.x = memData[7];
00120     msg_imu_.linear_acceleration.y = memData[8];
00121     msg_imu_.linear_acceleration.z = memData[9];
00122 
00123     // Covariances unknown
00124     msg_imu_.orientation_covariance[0] = -1;
00125     msg_imu_.angular_velocity_covariance[0] = -1;
00126     msg_imu_.linear_acceleration_covariance[0] = -1;
00127 
00128 
00129     for_each( message_actions::MessageAction action, actions )
00130     {
00131       callbacks_[action]( msg_imu_ );
00132     }
00133   }
00134 
00135 }
00136 
00137 }


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