PoseInfoContainer.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include "PoseInfoContainer.h"
00030 
00031 void PoseInfoContainer::update(const Eigen::Vector3f& slamPose, const Eigen::Matrix3f& slamCov, const ros::Time& stamp, const std::string& frame_id)
00032 {
00033   //Fill stampedPose
00034   std_msgs::Header& header = stampedPose_.header;
00035   header.stamp = stamp;
00036   header.frame_id = frame_id;
00037 
00038   geometry_msgs::Pose& pose = stampedPose_.pose;
00039   pose.position.x = slamPose.x();
00040   pose.position.y = slamPose.y();
00041 
00042   pose.orientation.w = cos(slamPose.z()*0.5f);
00043   pose.orientation.z = sin(slamPose.z()*0.5f);
00044 
00045   //Fill covPose
00046   //geometry_msgs::PoseWithCovarianceStamped covPose;
00047   covPose_.header = header;
00048   covPose_.pose.pose = pose;
00049 
00050   boost::array<double, 36>& cov(covPose_.pose.covariance);
00051 
00052   cov[0] = static_cast<double>(slamCov(0,0));
00053   cov[7] = static_cast<double>(slamCov(1,1));
00054   cov[35] = static_cast<double>(slamCov(2,2));
00055 
00056   double xyC = static_cast<double>(slamCov(0,1));
00057   cov[1] = xyC;
00058   cov[6] = xyC;
00059 
00060   double xaC = static_cast<double>(slamCov(0,2));
00061   cov[5] = xaC;
00062   cov[30] = xaC;
00063 
00064   double yaC = static_cast<double>(slamCov(1,2));
00065   cov[11] = yaC;
00066   cov[31] = yaC;
00067 
00068   //Fill tf tansform
00069   tf::poseMsgToTF(pose, poseTransform_);
00070 }


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Mon Jun 27 2016 04:57:19