nao_sensors_cpp.cpp
Go to the documentation of this file.
00001 /*
00002 # ROS node to read Nao's sensors and torso odometry through the Aldebaran API.
00003 # This code is currently compatible to NaoQI version 1.6
00004 #
00005 # Copyright 2011 Daniel Maier, University of Freiburg
00006 # http://www.ros.org/wiki/nao
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions 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 copyright
00014 #       notice, this list of conditions and the following disclaimer in the
00015 #       documentation and/or other materials provided with the distribution.
00016 #    # Neither the name of the University of Freiburg nor the names of its
00017 #       contributors may be used to endorse or promote products derived from
00018 #       this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00021 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00022 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00023 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00024 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00025 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00026 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00027 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00028 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00029 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00030 # POSSIBILITY OF SUCH DAMAGE.
00031 */
00032 
00033 #include <ros/ros.h>
00034 #include <sensor_msgs/JointState.h>
00035 #include <geometry_msgs/Transform.h>
00036 #include <nav_msgs/Odometry.h>
00037 #include <sensor_msgs/Imu.h>
00038 #include <iostream>
00039 
00040 #include <std_srvs/Empty.h>
00041 #include <nao_msgs/SetTransform.h>
00042 
00043 #include <tf/transform_broadcaster.h>
00044 #include <tf/transform_datatypes.h>
00045 
00046 // Aldebaran includes
00047 #include <alproxies/almemoryproxy.h>
00048 #include <alproxies/almotionproxy.h>
00049 #include <alvision/alvisiondefinitions.h>
00050 #include <alerror/alerror.h>
00051 #include <alvalue/alvalue.h>
00052 #include <alcommon/altoolsmain.h>
00053 #include <alproxies/alvideodeviceproxy.h>
00054 #include <alcommon/albroker.h>
00055 #include <alcommon/albrokermanager.h>
00056 #include <alcommon/alproxy.h>
00057 #include <alproxies/alproxies.h>
00058 #include <alproxies/almotionproxy.h>
00059 #include <alproxies/almemoryproxy.h>
00060 #include <alcommon/almodule.h>
00061 #include <alcommon/albroker.h>
00062 #include <alcommon/albrokermanager.h>
00063 
00064 
00065 
00066 // Other includes
00067 #include <boost/program_options.hpp>
00068 
00069 using namespace std;
00070 
00071 class NaoNode
00072 {
00073    public:
00074       NaoNode();
00075       ~NaoNode();
00076       bool connectNaoQi();
00077       void parse_command_line(int argc, char ** argv);
00078    protected:
00079       std::string m_pip;
00080       std::string m_ip;
00081       int m_port;
00082       int m_pport;
00083       std::string m_brokerName;
00084       boost::shared_ptr<AL::ALBroker> m_broker;
00085 
00086 
00087 };
00088 
00089 
00090 NaoNode::NaoNode() : m_pip("127.0.0.01"),m_ip("0.0.0.0"),m_port(0),m_pport(9559),m_brokerName("NaoROSBroker")
00091 {
00092 
00093 
00094 }
00095 
00096 NaoNode::~NaoNode()
00097 {
00098 }
00099 
00100 void NaoNode::parse_command_line(int argc, char ** argv)
00101 {
00102    std::string pip;
00103    std::string ip;
00104    int pport;
00105    int port;
00106    boost::program_options::options_description desc("Configuration");
00107    desc.add_options()
00108       ("help", "show this help message")
00109       ("ip", boost::program_options::value<std::string>(&ip)->default_value(m_ip),
00110        "IP/hostname of the broker")
00111       ("port", boost::program_options::value<int>(&port)->default_value(m_port),
00112        "Port of the broker")
00113       ("pip", boost::program_options::value<std::string>(&pip)->default_value(m_pip),
00114        "IP/hostname of parent broker")
00115       ("pport", boost::program_options::value<int>(&pport)->default_value(m_pport),
00116        "port of parent broker")
00117       ;
00118    boost::program_options::variables_map vm;
00119    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
00120    boost::program_options::notify(vm);
00121    m_port = vm["port"].as<int>();
00122    m_pport = vm["pport"].as<int>();
00123    m_pip = vm["pip"].as<std::string>();
00124    m_ip = vm["ip"].as<std::string>();
00125    cout << "pip is " << m_pip << endl;
00126    cout << "ip is " << m_ip << endl;
00127    cout << "port is " << m_port << endl;
00128    cout << "pport is " << m_pport << endl;
00129 
00130    if (vm.count("help")) {
00131       std::cout << desc << "\n";
00132       return ;
00133    }
00134 }
00135 
00136 
00137 
00138 bool NaoNode::connectNaoQi()
00139 {
00140    // Need this to for SOAP serialization of floats to work
00141    setlocale(LC_NUMERIC, "C");
00142    // A broker needs a name, an IP and a port:
00143    // FIXME: would be a good idea to look for a free port first
00144    // listen port of the broker (here an anything)
00145    try
00146    {
00147       m_broker = AL::ALBroker::createBroker(m_brokerName, m_ip, m_port, m_pip, m_pport, false);
00148    }
00149    catch(const AL::ALError& e)
00150    {
00151       ROS_ERROR( "Failed to connect broker to: %s:%d",m_pip.c_str(),m_port);
00152       //AL::ALBrokerManager::getInstance()->killAllBroker();
00153       //AL::ALBrokerManager::kill();
00154       return false;
00155    }
00156    cout << "broker ready." << endl;
00157    return true;
00158 }
00159 
00160 class NaoSensors : public NaoNode
00161 {
00162 public:
00163 
00164     NaoSensors(int argc, char ** argv);
00165     ~NaoSensors();
00166 
00167     bool connectProxy();
00168     void run();
00169 
00170     bool pauseOdomCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00171     bool resumeOdomCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00172     bool odomOffsetCallback(nao_msgs::SetTransform::Request& req, nao_msgs::SetTransform::Response& res);
00173     bool setOdomPoseCallback(nao_msgs::SetTransform::Request& req, nao_msgs::SetTransform::Response& res);
00174 
00175 
00176 protected:
00177 
00178     double m_rate;
00179 
00180     // NAOqi
00181     boost::shared_ptr<AL::ALMotionProxy> m_motionProxy;
00182     boost::shared_ptr<AL::ALMemoryProxy> m_memoryProxy;
00183     AL::ALValue m_dataNamesList;
00184 
00185     // ROS
00186     ros::NodeHandle m_nh;
00187     ros::NodeHandle m_privateNh;
00188 
00189     // Services
00190     ros::ServiceServer m_pauseOdomSrv;
00191     ros::ServiceServer m_resumeOdomSrv;
00192     ros::ServiceServer m_odomOffsetSrv;
00193     ros::ServiceServer m_setOdomPoseSrv;
00194 
00195     std::string m_odomFrameId;
00196     std::string m_baseFrameId;
00197 
00198     nav_msgs::Odometry m_odom;
00199     sensor_msgs::Imu m_torsoIMU;
00200     sensor_msgs::JointState m_jointState;
00201 
00202     ros::Publisher m_odomPub;
00203     tf::TransformBroadcaster m_transformBroadcaster;
00204     ros::Publisher m_torsoIMUPub;
00205     ros::Publisher m_jointStatePub;
00206 
00207     // Odometry-specific members
00208     geometry_msgs::TransformStamped m_odomTransformMsg;
00209     tf::Pose m_odomPose; // current "real" odometry pose in original (Nao) odom frame
00210     tf::Transform m_odomOffset; // offset on odometry origin
00211 
00212     bool m_useIMUAngles;
00213 
00214     bool m_paused;
00215     double m_lastOdomTime;
00216 
00217     tf::Pose m_targetPose;
00218     bool m_mustUpdateOffset;
00219     bool m_initializeFromIMU;
00220     bool m_initializeFromOdometry;
00221     bool m_isInitialized;
00222 };
00223 
00224 bool NaoSensors::connectProxy()
00225 {
00226    if (!m_broker)
00227    {
00228       ROS_ERROR("Broker is not ready. Have you called connectNaoQi()?");
00229       return false;
00230    }
00231    try
00232    {
00233       //m_motionProxy = boost::shared_ptr<AL::ALProxy>(m_broker->getProxy("ALMotion"));
00234       m_motionProxy = boost::shared_ptr<AL::ALMotionProxy>(new AL::ALMotionProxy(m_broker));
00235    }
00236    catch (const AL::ALError& e)
00237    {
00238       ROS_ERROR("Could not create ALMotionProxy.");
00239       return false;
00240    }
00241    try
00242    {
00243       //m_memoryProxy = boost::shared_ptr<AL::ALMemoryProxy>(m_broker->getProxy("ALMemory"));
00244       m_memoryProxy = boost::shared_ptr<AL::ALMemoryProxy>(new AL::ALMemoryProxy(m_broker));
00245    }
00246    catch (const AL::ALError& e)
00247    {
00248       ROS_ERROR("Could not create ALMemoryProxy.");
00249       return false;
00250    }
00251    ROS_INFO("Proxies to ALMotion and ALMemory ready.");
00252    return true;
00253 }
00254 
00255 NaoSensors::NaoSensors(int argc, char ** argv)
00256  : m_rate(25.0), m_privateNh("~"),
00257    m_odomFrameId("odom"),
00258    m_baseFrameId("base_link"),
00259    m_useIMUAngles(false),
00260    m_paused(false),
00261    m_lastOdomTime(0.0),
00262    m_mustUpdateOffset(false), m_initializeFromIMU(false),
00263    m_initializeFromOdometry(false), m_isInitialized(false)
00264 {
00265     parse_command_line(argc,argv);
00266     if (   ! connectNaoQi() || !  connectProxy() )
00267     {
00268       ROS_ERROR("Gosh! Throwsing exception");
00269       throw std::exception();
00270     }
00271     m_dataNamesList = AL::ALValue::array("DCM/Time",
00272          "Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value","Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value",
00273          "Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value",
00274          "Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value", "Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value", 
00275          "Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value",
00276          "Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value", "Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value",
00277          "Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value");
00278 
00279 
00280     // get update frequency. default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
00281     m_privateNh.param("sensor_rate", m_rate,m_rate);
00282 
00283     // get base_frame_id (and fix prefix if necessary)
00284     m_privateNh.param("base_frame_id", m_baseFrameId, m_baseFrameId);
00285     m_privateNh.param("odom_frame_id", m_odomFrameId, m_odomFrameId);
00286     m_privateNh.param("use_imu_angles", m_useIMUAngles, m_useIMUAngles);
00287 
00288     m_pauseOdomSrv = m_nh.advertiseService("pause_odometry", &NaoSensors::pauseOdomCallback, this);
00289     m_resumeOdomSrv = m_nh.advertiseService("resume_odometry", &NaoSensors::resumeOdomCallback, this);
00290     m_odomOffsetSrv = m_nh.advertiseService("odometry_offset", &NaoSensors::odomOffsetCallback, this);
00291     m_setOdomPoseSrv = m_nh.advertiseService("set_odometry_pose", &NaoSensors::setOdomPoseCallback, this);
00292 
00293 
00294     m_odom.header.frame_id = m_odomFrameId;
00295     m_torsoIMU.header.frame_id = m_baseFrameId;
00296     m_jointState.name = m_motionProxy->getJointNames("Body");
00297 
00298     // simluated model misses some joints, we need to fill:
00299     if (m_jointState.name.size() == 22)
00300     {
00301       // TODO: Check this!
00302       m_jointState.name.insert(m_jointState.name.begin()+6,"LWristYaw");
00303       m_jointState.name.insert(m_jointState.name.begin()+7,"LHand");
00304       m_jointState.name.push_back("RWristYaw");
00305       m_jointState.name.push_back("RHand");
00306     }
00307 
00308     std::stringstream ss;
00309     std::copy(m_jointState.name.begin(), m_jointState.name.end()-1, std::ostream_iterator<std::string>(ss,","));
00310     std::copy(m_jointState.name.end()-1, m_jointState.name.end(), std::ostream_iterator<std::string>(ss));
00311     ROS_INFO("Nao joints found: %s",ss.str().c_str());
00312 
00313     if (m_useIMUAngles)
00314         ROS_INFO("Using IMU for odometry roll & pitch");
00315 
00316 
00317     // default values of transforms:
00318     m_odomTransformMsg.header.frame_id = m_odomFrameId;
00319     m_odomTransformMsg.child_frame_id = m_baseFrameId;
00320 
00321     m_odomOffset = tf::Transform(tf::createIdentityQuaternion());
00322     m_odomPose = tf::Transform(tf::createIdentityQuaternion());
00323 
00324     m_odomPub = m_nh.advertise<nav_msgs::Odometry>("odom",5);
00325     m_torsoIMUPub = m_nh.advertise<sensor_msgs::Imu>("imu",5);
00326     m_jointStatePub = m_nh.advertise<sensor_msgs::JointState>("joint_states",5);
00327 
00328     ROS_INFO("nao_sensors initialized");
00329 
00330 }
00331 NaoSensors::~NaoSensors()
00332 {
00333 }
00334 void NaoSensors::run()
00335 {
00336    ros::Rate r(m_rate);
00337    ros::Time stamp1;
00338    ros::Time stamp2;
00339    ros::Time stamp;
00340 
00341    std::vector<float> odomData;
00342    float odomX, odomY, odomZ, odomWX, odomWY, odomWZ;
00343    
00344    std::vector<float> memData;
00345    
00346    std::vector<float> positionData;
00347 
00348    ROS_INFO("Staring main loop. ros::ok() is %d nh.ok() is %d",ros::ok(),m_nh.ok());
00349    while(ros::ok() )
00350    {
00351       r.sleep();
00352       ros::spinOnce();
00353       stamp1 = ros::Time::now();
00354       try
00355       {
00356          memData = m_memoryProxy->getListData(m_dataNamesList);
00357          // {SPACE_TORSO = 0, SPACE_WORLD = 1, SPACE_NAO = 2}. (second argument)
00358          odomData = m_motionProxy->getPosition("Torso", 1, true);
00359          positionData = m_motionProxy->getAngles("Body", true);
00360       }
00361       catch (const AL::ALError & e)
00362       {
00363          ROS_ERROR( "Error accessing ALMemory, exiting...");
00364          ROS_ERROR( "%s", e.what() );
00365          //ros.signal_shutdown("No NaoQI available anymore");
00366       }
00367       stamp2 = ros::Time::now();
00368       //ROS_DEBUG("dt is %f",(stamp2-stamp1).toSec()); % dt is typically around 1/1000 sec
00369       // TODO: Something smarter than this..
00370       stamp = stamp1 + ros::Duration((stamp2-stamp1).toSec()/2.0);
00371 
00372       /******************************************************************
00373        *                              IMU
00374        *****************************************************************/
00375       if (memData.size() != m_dataNamesList.getSize())
00376       {
00377          ROS_ERROR("memData length %zu does not match expected length %u",memData.size(),m_dataNamesList.getSize() );
00378          continue;
00379       }
00380       // IMU data:
00381       m_torsoIMU.header.stamp = stamp;
00382 
00383       float angleX = memData[1];
00384       float angleY = memData[2];
00385       float angleZ = memData[3];
00386       float gyroX = memData[4];
00387       float gyroY = memData[5];
00388       float gyroZ = memData[6];
00389       float accX = memData[7];
00390       float accY = memData[8];
00391       float accZ = memData[9];
00392 
00393       m_torsoIMU.orientation = tf::createQuaternionMsgFromRollPitchYaw(
00394                                 angleX,
00395                                 angleY,
00396                                 angleZ); // yaw currently always 0
00397 
00398       m_torsoIMU.angular_velocity.x = gyroX;
00399       m_torsoIMU.angular_velocity.y = gyroY;
00400       m_torsoIMU.angular_velocity.z = gyroZ; // currently always 0
00401 
00402       m_torsoIMU.linear_acceleration.x = accX;
00403       m_torsoIMU.linear_acceleration.y = accY;
00404       m_torsoIMU.linear_acceleration.z = accZ;
00405 
00406       // covariances unknown
00407       // cf http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html
00408       m_torsoIMU.orientation_covariance[0] = -1;
00409       m_torsoIMU.angular_velocity_covariance[0] = -1;
00410       m_torsoIMU.linear_acceleration_covariance[0] = -1;
00411 
00412       m_torsoIMUPub.publish(m_torsoIMU);
00413 
00414 
00415       /******************************************************************
00416        *                            Joint state
00417        *****************************************************************/
00418       m_jointState.header.stamp = stamp;
00419       m_jointState.header.frame_id = m_baseFrameId;
00420       m_jointState.position.resize(positionData.size());
00421       for(unsigned i = 0; i<positionData.size(); ++i)
00422       {
00423          m_jointState.position[i] = positionData[i];
00424       }
00425 
00426       // simulated model misses some joints, we need to fill:
00427       if (m_jointState.position.size() == 22)
00428       {
00429          m_jointState.position.insert(m_jointState.position.begin()+6,0.0);
00430          m_jointState.position.insert(m_jointState.position.begin()+7,0.0);
00431          m_jointState.position.push_back(0.0);
00432          m_jointState.position.push_back(0.0);
00433       }
00434 
00435       m_jointStatePub.publish(m_jointState);
00436 
00437 
00438         /******************************************************************
00439         *                            Odometry
00440         *****************************************************************/
00441         if (!m_paused) {
00442 
00443         // apply offset transformation:
00444         tf::Pose transformedPose;
00445 
00446 
00447         if (odomData.size()!=6)
00448         {
00449             ROS_ERROR( "Error getting odom data. length is %zu",odomData.size() );
00450             continue;
00451         }
00452 
00453         double dt = (stamp.toSec() - m_lastOdomTime);
00454 
00455         odomX = odomData[0];
00456         odomY = odomData[1];
00457         odomZ = odomData[2];
00458         odomWX = odomData[3];
00459         odomWY = odomData[4];
00460         odomWZ = odomData[5];
00461 
00462         tf::Quaternion q;
00463         // roll and pitch from IMU, yaw from odometry:
00464         if (m_useIMUAngles)
00465             q.setRPY(angleX, angleY, odomWZ);
00466         else
00467             q.setRPY(odomWX, odomWY, odomWZ);
00468 
00469         m_odomPose.setOrigin(tf::Vector3(odomX, odomY, odomZ));
00470         m_odomPose.setRotation(q);
00471 
00472         if(m_mustUpdateOffset) {
00473             if(!m_isInitialized) {
00474                 if(m_initializeFromIMU) {
00475                     // Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU
00476                     m_targetPose.setOrigin(m_odomPose.getOrigin());
00477                     m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, odomWZ));
00478                 } else if(m_initializeFromOdometry) {
00479                     m_targetPose.setOrigin(m_odomPose.getOrigin());
00480                     m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, odomWZ));
00481                 }
00482                 m_isInitialized = true;
00483             } else {
00484                 // Overwrite target pitch and roll angles with IMU data
00485                 const double target_yaw = tf::getYaw(m_targetPose.getRotation());
00486                 if(m_initializeFromIMU) {
00487                     m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, target_yaw));
00488                 } else if(m_initializeFromOdometry){
00489                     m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, target_yaw));
00490                 }
00491             }
00492             m_odomOffset = m_targetPose * m_odomPose.inverse();
00493             transformedPose = m_targetPose;
00494             m_mustUpdateOffset = false;
00495         } else {
00496             transformedPose = m_odomOffset * m_odomPose;
00497         }
00498 
00499         //
00500         // publish the transform over tf first
00501         //
00502         m_odomTransformMsg.header.stamp = stamp;
00503         tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform);
00504         m_transformBroadcaster.sendTransform(m_odomTransformMsg);
00505 
00506 
00507         //
00508         // Fill the Odometry msg
00509         //
00510         m_odom.header.stamp = stamp;
00511         //set the velocity first (old values still valid)
00512         m_odom.twist.twist.linear.x = (odomX - m_odom.pose.pose.position.x) / dt;
00513         m_odom.twist.twist.linear.y = (odomY - m_odom.pose.pose.position.y) / dt;
00514         m_odom.twist.twist.linear.z = (odomZ - m_odom.pose.pose.position.z) / dt;
00515         
00516         // TODO: calc angular velocity!
00517         //      m_odom.twist.twist.angular.z = vth; ??
00518 
00519         //set the position from the above calculated transform
00520         m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation;
00521         m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x;
00522         m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y;
00523         m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z;
00524 
00525 
00526         m_odomPub.publish(m_odom);
00527 
00528 
00529         m_lastOdomTime = stamp.toSec();
00530 
00531         }
00532     }
00533     ROS_INFO("nao_sensors stopped.");
00534 
00535 }
00536 
00537 bool NaoSensors::pauseOdomCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res){
00538     if (m_paused){
00539         ROS_WARN("Odometry pause requested, but is already paused");
00540         return false;
00541     } else{
00542         ROS_INFO("Odometry paused");
00543         m_paused = true;
00544         m_targetPose = m_odomOffset * m_odomPose;
00545         m_mustUpdateOffset = true;
00546         return true;
00547     }
00548 }
00549 
00550 bool NaoSensors::resumeOdomCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res){
00551     if (m_paused){
00552         ROS_INFO("Odometry resumed");
00553         m_paused = false;
00554         return true;
00555     } else{
00556         ROS_WARN("Odometry resume requested, but is not paused");
00557         return false;
00558     }
00559 }
00560 
00561 bool NaoSensors::odomOffsetCallback(nao_msgs::SetTransform::Request& req, nao_msgs::SetTransform::Response& res){
00562     ROS_INFO("New odometry offset received");
00563     tf::Transform newOffset;
00564     tf::transformMsgToTF(req.offset, newOffset);
00565 
00566     // add new offset to current (transformed) odom pose:
00567     if(!m_mustUpdateOffset) {
00568         m_mustUpdateOffset = true;
00569         m_targetPose = m_odomOffset * m_odomPose * newOffset;
00570     } else {
00571         m_targetPose = m_targetPose * newOffset;
00572     }
00573 
00574     return true;
00575 }
00576 
00577 bool NaoSensors::setOdomPoseCallback(nao_msgs::SetTransform::Request& req, nao_msgs::SetTransform::Response& res){
00578     ROS_INFO("New target for current odometry pose received");
00579     tf::Transform targetPose;
00580     tf::transformMsgToTF(req.offset, targetPose);
00581 
00582     m_odomOffset = targetPose * m_odomPose.inverse();
00583 
00584     return true;
00585 }
00586 
00587 int main(int argc, char ** argv)
00588 {
00589    ros::init(argc, argv, "nao_sensors_cpp");
00590    cout << "I am here" << endl;
00591    NaoSensors * sensors;
00592    try{
00593       sensors = new NaoSensors(argc,argv);
00594       //NaoSensors sensors(argc,argv);
00595    }
00596    catch (const std::exception & e)
00597       //catch (...)
00598    {
00599       // TODO: why does this not work?
00600       //ROS_ERROR("Creating NaoSensors object failed with error %s",e.what());
00601       ROS_ERROR("Creating NaoSensors object failed with error ");
00602       return -1;
00603    }
00604    
00605    sensors->run();
00606    
00607    delete sensors;
00608 
00609    ROS_INFO("nao_sensors stopped");
00610    return 0;
00611 }


nao_driver
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Séverin Lemaignan
autogenerated on Thu Oct 30 2014 09:20:04