pr2_plugs_common: pr2_plugs_common: covariance

00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Wim Meeussen
00036 *********************************************************************/
00037 
00038 
00039 #include <ros/ros.h>
00040 #include <tf/tf.h>
00041 #include <tf/transform_listener.h>
00042 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00043 
00044 
00045 static std::string fixed_frame_;
00046 static std::vector<double> cov_fixed_;
00047 
00048 
00049 
00050 void poseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose)
00051 {
00052   geometry_msgs::PoseWithCovarianceStamped pose_fixed;
00053   ros::NodeHandle node_top;
00054   static tf::TransformListener tf;
00055   static  ros::Publisher  pub = node_top.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose_out", 10);
00056 
00057   // transfrom pose to fixed frame
00058   ROS_DEBUG("Measurement in frame %s: %f, %f, %f,     %f, %f, %f, %f",
00059             pose->header.frame_id.c_str(), 
00060             pose->pose.pose.position.x, 
00061             pose->pose.pose.position.y,
00062             pose->pose.pose.position.z,
00063             pose->pose.pose.orientation.x,
00064             pose->pose.pose.orientation.y,
00065             pose->pose.pose.orientation.z,
00066             pose->pose.pose.orientation.w);
00067 
00068   // convert posewithcovariancestamped to posestamped
00069   tf::Stamped<tf::Pose> tf_stamped_pose;
00070   tf::poseMsgToTF(pose->pose.pose, tf_stamped_pose);
00071   tf_stamped_pose.stamp_ = pose->header.stamp;
00072   tf_stamped_pose.frame_id_ = pose->header.frame_id;
00073 
00074   // transform posestamped to fixed frame
00075   if (!tf.waitForTransform(fixed_frame_, pose->header.frame_id, pose->header.stamp, ros::Duration(0.5))){
00076     ROS_ERROR("Could not transform from %s to %s at time %f", fixed_frame_.c_str(), pose->header.frame_id.c_str(), pose->header.stamp.toSec());
00077     return;
00078   }
00079   tf.transformPose(fixed_frame_, tf_stamped_pose, tf_stamped_pose);
00080 
00081   // convert posestamped back to posewithcovariancestamped
00082   pose_fixed = *pose;
00083   tf::poseTFToMsg(tf_stamped_pose, pose_fixed.pose.pose);
00084   pose_fixed.header.frame_id = fixed_frame_;
00085 
00086   ROS_DEBUG("Measurement in frame %s: %f, %f, %f,     %f, %f, %f, %f",
00087             fixed_frame_.c_str(), 
00088             pose_fixed.pose.pose.position.x, 
00089             pose_fixed.pose.pose.position.y,
00090             pose_fixed.pose.pose.position.z,
00091             pose_fixed.pose.pose.orientation.x,
00092             pose_fixed.pose.pose.orientation.y,
00093             pose_fixed.pose.pose.orientation.z,
00094             pose_fixed.pose.pose.orientation.w);
00095 
00096   // set fixed frame covariance
00097   for (unsigned int i=0; i<6; i++)
00098     pose_fixed.pose.covariance[6*i+i] = cov_fixed_[i];
00099   pub.publish(pose_fixed);
00100 }
00101 
00102 
00103 
00104 int main(int argc, char** argv){
00105   ros::init(argc, argv, "covariance_inserter");
00106   ros::NodeHandle node("~");
00107   cov_fixed_.resize(6);
00108 
00109   node.param("fixed_frame", fixed_frame_, std::string("fixed_frame_not_specified"));
00110   node.param("cov_fixed1", cov_fixed_[0], 0.0);
00111   node.param("cov_fixed2", cov_fixed_[1], 0.0);
00112   node.param("cov_fixed3", cov_fixed_[2], 0.0);
00113   node.param("cov_fixed4", cov_fixed_[3], 0.0);
00114   node.param("cov_fixed5", cov_fixed_[4], 0.0);
00115   node.param("cov_fixed6", cov_fixed_[5], 0.0);
00116 
00117   ros::NodeHandle node_top;
00118   ros::Subscriber sub = node_top.subscribe<geometry_msgs::PoseWithCovarianceStamped>("pose_in", 10, &poseCallback);
00119 
00120   ros::spin();
00121   return 0;
00122 }
covariance_inserter.cpp
