remap_odometry.cpp
Go to the documentation of this file.
00001 // SVN $HeadURL$
00002 // SVN $Id$
00003 
00004 /*
00005  * Nao Odometry Remapping in ROS (based on odometry_publisher_tutorial)
00006  *
00007  * Copyright 2009-2011 Armin Hornung, University of Freiburg
00008  * http://www.ros.org/wiki/nao
00009  *
00010  * Redistribution and use in source and binary forms, with or without
00011  * modification, are permitted provided that the following conditions are met:
00012  *
00013  *     * Redistributions of source code must retain the above copyright
00014  *       notice, this list of conditions and the following disclaimer.
00015  *     * Redistributions in binary form must reproduce the above copyright
00016  *       notice, this list of conditions and the following disclaimer in the
00017  *       documentation and/or other materials provided with the distribution.
00018  *     * Neither the name of the University of Freiburg nor the names of its
00019  *       contributors may be used to endorse or promote products derived from
00020  *       this software without specific prior written permission.
00021  *
00022  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00023  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00024  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00025  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00026  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00027  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00028  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00029  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00030  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00031  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  * POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #include <ros/ros.h>
00036 #include <message_filters/subscriber.h>
00037 #include <message_filters/time_synchronizer.h>
00038 #include <tf/transform_broadcaster.h>
00039 #include <tf/transform_datatypes.h>
00040 #include <tf/transform_listener.h>
00041 #include <tf/message_filter.h>
00042 #include <nav_msgs/Odometry.h>
00043 #include <geometry_msgs/Transform.h>
00044 #include <std_srvs/Empty.h>
00045 #include <sensor_msgs/JointState.h>
00046 #include <nao_msgs/TorsoOdometry.h>
00047 #include <nao_msgs/TorsoIMU.h>
00048 #include <nao_remote/SetTransform.h>
00049 #include <cmath>
00050 
00051 class OdometryRemap
00052 {
00053 public:
00054         OdometryRemap();
00055         ~OdometryRemap();
00056         bool pauseOdomCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00057         bool resumeOdomCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00058         bool odomOffsetCallback(nao_remote::SetTransform::Request& req, nao_remote::SetTransform::Response& res);
00059         bool setOdomPoseCallback(nao_remote::SetTransform::Request& req, nao_remote::SetTransform::Response& res);
00060 
00061 
00062 private:
00063         void jsCallback(const sensor_msgs::JointState::ConstPtr & msg);
00064         void torsoOdomCallback(const nao_msgs::TorsoOdometryConstPtr& odom, const nao_msgs::TorsoIMUConstPtr& imu);
00065 
00066         ros::Publisher m_odomPub;
00067         tf::TransformBroadcaster m_transformBroadcaster;
00068         message_filters::Subscriber<nao_msgs::TorsoOdometry>* m_torsoOdomSub;
00069         message_filters::Subscriber<nao_msgs::TorsoIMU>* m_torsoIMUSub;
00070         message_filters::TimeSynchronizer<nao_msgs::TorsoOdometry, nao_msgs::TorsoIMU>* m_synchronizer;
00071         ros::ServiceServer m_pauseOdomSrv;
00072         ros::ServiceServer m_resumeOdomSrv;
00073         ros::ServiceServer m_odomOffsetSrv;
00074         ros::ServiceServer m_setOdomPoseSrv;
00075         ros::NodeHandle m_nh;
00076         ros::NodeHandle m_privateNh;
00077 
00078         //for base_footprint_frame: broadcasts frame between right and left foot, coinciding with the ground
00079         message_filters::Subscriber<sensor_msgs::JointState> * m_jsSub;
00080         tf::MessageFilter<sensor_msgs::JointState> * m_jsFilter;
00081         tf::TransformBroadcaster m_brBaseFootPrint;
00082         tf::TransformListener m_listener;
00083 
00084         geometry_msgs::TransformStamped m_odomTransformMsg;
00085         tf::Pose m_odomPose; // current "real" odometry pose in original (Nao) odom frame
00086         tf::Transform m_odomOffset; // offset on odometry origin
00087         nav_msgs::Odometry m_odom;
00088 
00089         std::string m_odomFrameId;
00090         std::string m_baseFrameId;
00091         std::string m_lfootFrameId;
00092         std::string m_rfootFrameId;
00093         std::string m_imuTopic;
00094         std::string m_baseFootPrintID;
00095         bool m_useIMUAngles;
00096         bool m_paused;
00097         double m_lastOdomTime;
00098 
00099         double m_lastWx;
00100         double m_lastWy;
00101         double m_lastWz;
00102 
00103         tf::Pose m_targetPose;
00104         bool m_mustUpdateOffset;
00105         bool m_initializeFromIMU;
00106         bool m_initializeFromOdometry;
00107         bool m_isInitialized;
00108 };
00109 
00110 OdometryRemap::OdometryRemap()
00111   : m_privateNh("~"), m_odomFrameId("odom"), m_baseFrameId("base_link"),
00112     m_lfootFrameId("l_sole"), m_rfootFrameId("r_sole"),
00113     m_imuTopic("torso_imu"), m_baseFootPrintID("base_footprint"),
00114     m_useIMUAngles(false), m_paused(false),
00115     m_lastOdomTime(0.0), m_lastWx(0.0), m_lastWy(0.0), m_lastWz(0.0),
00116     m_mustUpdateOffset(false), m_initializeFromIMU(false),
00117     m_initializeFromOdometry(false), m_isInitialized(false)
00118 {
00119     // Read parameters
00120         m_privateNh.param("odom_frame_id", m_odomFrameId, m_odomFrameId);
00121         m_privateNh.param("base_frame_id", m_baseFrameId, m_baseFrameId);
00122         m_privateNh.param("use_imu_angles", m_useIMUAngles, m_useIMUAngles);
00123         m_privateNh.param("init_from_imu", m_initializeFromIMU, m_initializeFromIMU);
00124         m_privateNh.param("init_from_odometry", m_initializeFromOdometry, m_initializeFromOdometry);
00125         m_privateNh.param("imu_topic", m_imuTopic, m_imuTopic);
00126 
00127         // Resolve TF frames using ~tf_prefix parameter
00128         m_odomFrameId = m_listener.resolve(m_odomFrameId);
00129         m_baseFrameId = m_listener.resolve(m_baseFrameId);
00130         m_baseFootPrintID = m_listener.resolve(m_baseFootPrintID);
00131         m_lfootFrameId = m_listener.resolve(m_lfootFrameId);
00132         m_rfootFrameId = m_listener.resolve(m_rfootFrameId);
00133 
00134         m_odomPub = m_nh.advertise<nav_msgs::Odometry>("odom", 50);
00135 
00136         m_pauseOdomSrv = m_nh.advertiseService("pause_odometry", &OdometryRemap::pauseOdomCallback, this);
00137         m_resumeOdomSrv = m_nh.advertiseService("resume_odometry", &OdometryRemap::resumeOdomCallback, this);
00138         m_odomOffsetSrv = m_nh.advertiseService("odometry_offset", &OdometryRemap::odomOffsetCallback, this);
00139         m_setOdomPoseSrv = m_nh.advertiseService("set_odometry_pose", &OdometryRemap::setOdomPoseCallback, this);
00140 
00141         m_torsoOdomSub = new message_filters::Subscriber<nao_msgs::TorsoOdometry>(m_nh, "torso_odometry", 10);
00142         m_torsoIMUSub = new message_filters::Subscriber<nao_msgs::TorsoIMU>(m_nh, m_imuTopic, 10);
00143         m_synchronizer = new message_filters::TimeSynchronizer<nao_msgs::TorsoOdometry, nao_msgs::TorsoIMU>(*m_torsoOdomSub, *m_torsoIMUSub, 10);
00144         m_synchronizer->registerCallback(boost::bind(&OdometryRemap::torsoOdomCallback, this, _1, _2));
00145 
00146         m_jsSub = new message_filters::Subscriber<sensor_msgs::JointState>(m_nh, "joint_states", 50);
00147         m_jsFilter = new tf::MessageFilter<sensor_msgs::JointState>(*m_jsSub, m_listener, m_rfootFrameId, 50);
00148         std::vector<std::string> frames;
00149         frames.push_back(m_rfootFrameId);
00150         frames.push_back(m_odomFrameId);
00151         m_jsFilter->setTargetFrames(frames);
00152         m_jsFilter->registerCallback(boost::bind(&OdometryRemap::jsCallback, this, _1));
00153 
00154         if (m_useIMUAngles)
00155                 ROS_INFO("Using IMU for odometry roll & pitch");
00156 
00157         // default values of transforms:
00158         m_odomTransformMsg.header.frame_id = m_odomFrameId;
00159         m_odomTransformMsg.child_frame_id = m_baseFrameId;
00160 
00161         m_odomOffset = tf::Transform(tf::createIdentityQuaternion());
00162         m_odomPose = tf::Transform(tf::createIdentityQuaternion());
00163 
00164         if(m_initializeFromIMU || m_initializeFromOdometry) {
00165             m_mustUpdateOffset = true;
00166         } else {
00167             m_mustUpdateOffset = false;
00168             m_isInitialized = true;
00169         }
00170 }
00171 
00172 OdometryRemap::~OdometryRemap(){
00173         delete m_synchronizer;
00174         delete m_torsoOdomSub;
00175         delete m_torsoIMUSub;
00176         delete m_jsFilter;
00177         delete m_jsSub;
00178 }
00179 
00180 void OdometryRemap::torsoOdomCallback(const nao_msgs::TorsoOdometryConstPtr& msgOdom, const nao_msgs::TorsoIMUConstPtr& msgIMU){
00181         if (m_paused){
00182                 ROS_DEBUG("Skipping odometry callback, paused");
00183                 return;
00184         }
00185 
00186         double odomTime = (msgOdom->header.stamp).toSec();
00187         ROS_DEBUG("Received [%f %f %f %f (%f/%f) (%f/%f) %f]", odomTime, msgOdom->x, msgOdom->y, msgOdom->z, msgOdom->wx, msgIMU->angleX, msgOdom->wy, msgIMU->angleY, msgOdom->wz);
00188 
00189         double dt = (odomTime - m_lastOdomTime);
00190 
00191 
00192         tf::Quaternion q;
00193         // roll and pitch from IMU, yaw from odometry:
00194         if (m_useIMUAngles)
00195                 q.setRPY(msgIMU->angleX, msgIMU->angleY, msgOdom->wz);
00196         else
00197                 q.setRPY(msgOdom->wx, msgOdom->wy, msgOdom->wz);
00198 
00199 
00200         m_odomPose.setOrigin(tf::Vector3(msgOdom->x, msgOdom->y, msgOdom->z));
00201         m_odomPose.setRotation(q);
00202 
00203         // apply offset transformation:
00204         tf::Pose transformedPose;
00205 
00206         if(m_mustUpdateOffset) {
00207         if(!m_isInitialized) {
00208             if(m_initializeFromIMU) {
00209                 // Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU
00210                 m_targetPose.setOrigin(m_odomPose.getOrigin());
00211                 m_targetPose.setRotation(tf::createQuaternionFromRPY(msgIMU->angleX, msgIMU->angleY, msgOdom->wz));
00212             } else if(m_initializeFromOdometry) {
00213                 m_targetPose.setOrigin(m_odomPose.getOrigin());
00214                 m_targetPose.setRotation(tf::createQuaternionFromRPY(msgOdom->wx, msgOdom->wy, msgOdom->wz));
00215             }
00216             m_isInitialized = true;
00217         } else {
00218             // Overwrite target pitch and roll angles with IMU data
00219             const double target_yaw = tf::getYaw(m_targetPose.getRotation());
00220             if(m_initializeFromIMU) {
00221                 m_targetPose.setRotation(tf::createQuaternionFromRPY(msgIMU->angleX, msgIMU->angleY, target_yaw));
00222             } else if(m_initializeFromOdometry){
00223                 m_targetPose.setRotation(tf::createQuaternionFromRPY(msgOdom->wx, msgOdom->wy, target_yaw));
00224             }
00225         }
00226                 m_odomOffset = m_targetPose * m_odomPose.inverse();
00227                 transformedPose = m_targetPose;
00228                 m_mustUpdateOffset = false;
00229         } else {
00230             transformedPose = m_odomOffset * m_odomPose;
00231         }
00232 
00233         // publish the transform over tf first:
00234         m_odomTransformMsg.header.stamp = msgOdom->header.stamp;
00235         tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform);
00236         m_transformBroadcaster.sendTransform(m_odomTransformMsg);
00237 
00238         //next, publish the actual odometry message:
00239         m_odom.header.stamp = msgOdom->header.stamp;
00240         m_odom.header.frame_id = m_odomFrameId;
00241 
00242 
00243         //set the velocity first (old values still valid)
00244         m_odom.twist.twist.linear.x = (msgOdom->x - m_odom.pose.pose.position.x) / dt;
00245         m_odom.twist.twist.linear.y = (msgOdom->y - m_odom.pose.pose.position.y) / dt;
00246         m_odom.twist.twist.linear.z = (msgOdom->z - m_odom.pose.pose.position.z) / dt;
00247         // TODO: calc angular velocity!
00248 //      m_odom.twist.twist.angular.z = vth; ??
00249 
00250         //set the position from the above calculated transform
00251         m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation;
00252         m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x;
00253         m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y;
00254         m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z;
00255 
00256         //publish the message
00257         m_odomPub.publish(m_odom);
00258 
00259         m_lastOdomTime = odomTime;
00260 
00261         // TODO: not used
00262 //      m_lastWx = msgOdom->wx;
00263 //      m_lastWy = msgOdom->wy;
00264 //      m_lastWz = msgOdom->wz;
00265 }
00266 
00267 bool OdometryRemap::pauseOdomCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res){
00268         if (m_paused){
00269                 ROS_WARN("Odometry pause requested, but is already paused");
00270                 return false;
00271         } else{
00272                 ROS_INFO("Odometry paused");
00273                 m_paused = true;
00274                 m_targetPose = m_odomOffset * m_odomPose;
00275                 m_mustUpdateOffset = true;
00276                 return true;
00277         }
00278 }
00279 
00280 bool OdometryRemap::resumeOdomCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res){
00281         if (m_paused){
00282                 ROS_INFO("Odometry resumed");
00283                 m_paused = false;
00284                 return true;
00285         } else{
00286                 ROS_WARN("Odometry resume requested, but is not paused");
00287                 return false;
00288         }
00289 }
00290 
00291 bool OdometryRemap::odomOffsetCallback(nao_remote::SetTransform::Request& req, nao_remote::SetTransform::Response& res){
00292         ROS_INFO("New odometry offset received");
00293         tf::Transform newOffset;
00294         tf::transformMsgToTF(req.offset, newOffset);
00295 
00296         // add new offset to current (transformed) odom pose:
00297         if(!m_mustUpdateOffset) {
00298             m_mustUpdateOffset = true;
00299             m_targetPose = m_odomOffset * m_odomPose * newOffset;
00300         } else {
00301             m_targetPose = m_targetPose * newOffset;
00302         }
00303 
00304     return true;
00305 }
00306 
00307 bool OdometryRemap::setOdomPoseCallback(nao_remote::SetTransform::Request& req, nao_remote::SetTransform::Response& res){
00308         ROS_INFO("New target for current odometry pose received");
00309         tf::Transform targetPose;
00310         tf::transformMsgToTF(req.offset, targetPose);
00311 
00312         m_odomOffset = targetPose * m_odomPose.inverse();
00313 
00314         return true;
00315 }
00316 void OdometryRemap::jsCallback(const sensor_msgs::JointState::ConstPtr & ptr)
00317 {
00318    ros::Time time = ptr->header.stamp;
00319    tf::StampedTransform tf_odom_to_base, tf_odom_to_left_foot, tf_odom_to_right_foot;
00320 
00321    ROS_DEBUG("JointState callback function, computing frame %s", m_baseFootPrintID.c_str());
00322    try {
00323       m_listener.lookupTransform(m_odomFrameId, m_lfootFrameId, time, tf_odom_to_left_foot);
00324       m_listener.lookupTransform(m_odomFrameId, m_rfootFrameId, time, tf_odom_to_right_foot);
00325       m_listener.lookupTransform(m_odomFrameId, m_baseFrameId,  time, tf_odom_to_base);
00326    } catch (const tf::TransformException& ex){
00327       ROS_ERROR("%s",ex.what());
00328       return ;
00329    }
00330 
00331    tf::Vector3 new_origin = (tf_odom_to_right_foot.getOrigin() + tf_odom_to_left_foot.getOrigin())/2.0; // middle of both feet
00332    double height = std::min(tf_odom_to_left_foot.getOrigin().getZ(), tf_odom_to_right_foot.getOrigin().getZ()); // fix to lowest foot
00333    new_origin.setZ(height);
00334 
00335    // adjust yaw according to torso orientation, all other angles 0 (= in z-plane)
00336    double roll, pitch, yaw;
00337    tf_odom_to_base.getBasis().getRPY(roll, pitch, yaw);
00338 
00339    tf::Transform tf_odom_to_footprint(tf::createQuaternionFromYaw(yaw), new_origin);
00340    tf::Transform tf_base_to_footprint = tf_odom_to_base.inverse() * tf_odom_to_footprint;
00341 
00342    // publish transform with parent m_baseFrameId and new child m_baseFootPrintID
00343    // i.e. transform from m_baseFrameId to m_baseFootPrintID
00344    m_brBaseFootPrint.sendTransform(tf::StampedTransform(tf_base_to_footprint, time, m_baseFrameId, m_baseFootPrintID));
00345    ROS_DEBUG("Published Transform %s --> %s", m_baseFrameId.c_str(), m_baseFootPrintID.c_str());
00346 
00347    return;
00348 }
00349 
00350 int main(int argc, char** argv){
00351   ros::init(argc, argv, "remap_odometry");
00352   OdometryRemap odomRemap;
00353   ros::spin();
00354 
00355   return 0;
00356 }
00357 
00358 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


nao_remote
Author(s): Armin Hornung
autogenerated on Sat Oct 26 2013 11:02:42