$search
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 <geometry_msgs/PoseWithCovarianceStamped.h> 00041 #include <kdl/frames.hpp> 00042 #include "pr2_plugs_common/detector_filter.h" 00043 00044 namespace detector{ 00045 00046 using namespace MatrixWrapper; 00047 using namespace BFL; 00048 00049 00050 DetectorFilter::DetectorFilter() 00051 : initialized_(false), 00052 prior_(NULL), 00053 filter_(NULL) 00054 { 00055 // create system model 00056 ColumnVector sysNoise_Mu(6); sysNoise_Mu = 0; 00057 SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0; 00058 Matrix A(6,6); A = 0; 00059 for (unsigned int i=1; i<=6; i++){ 00060 sysNoise_Cov(i,i) = pow(0.000000001,2); 00061 A(i,i) = 1.0; 00062 } 00063 Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov); 00064 sys_pdf_ = new LinearAnalyticConditionalGaussian(A, system_Uncertainty); 00065 sys_model_ = new LinearAnalyticSystemModelGaussianUncertainty(sys_pdf_); 00066 00067 // create measurement model 00068 ColumnVector measNoise_Mu(6); measNoise_Mu = 0; 00069 SymmetricMatrix measNoise_Cov(6); measNoise_Cov = 0; 00070 Matrix H(6,6); H = 0; 00071 for (unsigned int i=1; i<=6; i++){ 00072 measNoise_Cov(i,i) = 1; 00073 H(i,i) = 1.0; 00074 } 00075 Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov); 00076 meas_pdf_ = new LinearAnalyticConditionalGaussian(H, measurement_Uncertainty); 00077 meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_); 00078 00079 ros::NodeHandle node_private("~"); 00080 node_private.param("fixed_frame", fixed_frame_, std::string("fixed_frame_not_specified")); 00081 reset_srv_ = node_private.advertiseService("reset_state", &DetectorFilter::resetState, this); 00082 00083 ros::NodeHandle node; 00084 pose_sub_ = node.subscribe<geometry_msgs::PoseWithCovarianceStamped>("pose_in", 10, &DetectorFilter::poseCallback, this); 00085 00086 } 00087 00088 DetectorFilter::~DetectorFilter() 00089 { 00090 if (filter_) delete filter_; 00091 if (prior_) delete prior_; 00092 if (meas_model_) delete meas_model_; 00093 if (meas_pdf_) delete meas_pdf_; 00094 if (sys_model_) delete sys_model_; 00095 if (sys_pdf_) delete sys_pdf_; 00096 } 00097 00098 00099 00100 void DetectorFilter::poseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose) 00101 { 00102 ROS_INFO("Measurement in frame %s: %f, %f, %f, %f, %f, %f, %f", 00103 pose->header.frame_id.c_str(), 00104 pose->pose.pose.position.x, 00105 pose->pose.pose.position.y, 00106 pose->pose.pose.position.z, 00107 pose->pose.pose.orientation.x, 00108 pose->pose.pose.orientation.y, 00109 pose->pose.pose.orientation.z, 00110 pose->pose.pose.orientation.w); 00111 00112 // convert posewithcovariancestamped to posestamped 00113 tf::Stamped<tf::Pose> tf_stamped_pose; 00114 tf::poseMsgToTF(pose->pose.pose, tf_stamped_pose); 00115 tf_stamped_pose.stamp_ = pose->header.stamp; 00116 tf_stamped_pose.frame_id_ = pose->header.frame_id; 00117 00118 // transform posestamped to fixed frame 00119 if (!tf_.waitForTransform(fixed_frame_, pose->header.frame_id, pose->header.stamp, ros::Duration(0.5))){ 00120 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()); 00121 return; 00122 } 00123 tf_.transformPose(fixed_frame_, tf_stamped_pose, tf_stamped_pose); 00124 00125 // convert posestamped back to posewithcovariancestamped 00126 geometry_msgs::PoseWithCovarianceStamped tf_covariance_pose; 00127 tf_covariance_pose = *pose; 00128 tf::poseTFToMsg(tf_stamped_pose, tf_covariance_pose.pose.pose); 00129 tf_covariance_pose.header.frame_id = fixed_frame_; 00130 00131 ROS_INFO("Measurement in frame %s: %f, %f, %f, %f, %f, %f, %f", 00132 fixed_frame_.c_str(), 00133 tf_covariance_pose.pose.pose.position.x, 00134 tf_covariance_pose.pose.pose.position.y, 00135 tf_covariance_pose.pose.pose.position.z, 00136 tf_covariance_pose.pose.pose.orientation.x, 00137 tf_covariance_pose.pose.pose.orientation.y, 00138 tf_covariance_pose.pose.pose.orientation.z, 00139 tf_covariance_pose.pose.pose.orientation.w); 00140 00141 // initialize filter first time 00142 if (!initialized_){ 00143 ROS_INFO("Initializing detector filter"); 00144 initialize(tf_covariance_pose); 00145 } 00146 // update filter 00147 else{ 00148 ROS_DEBUG("update filter"); 00149 ColumnVector measurement(6); 00150 decomposeTransform(tf_covariance_pose, measurement); 00151 filter_->Update(sys_model_); 00152 filter_->Update(meas_model_, measurement); 00153 } 00154 } 00155 00156 void DetectorFilter::initialize(const geometry_msgs::PoseWithCovarianceStamped& pose) 00157 { 00158 // set prior of filter 00159 ColumnVector prior_Mu(6); 00160 decomposeTransform(pose, prior_Mu); 00161 SymmetricMatrix prior_Cov(6); 00162 for (unsigned int i=0; i<6; i++) 00163 for (unsigned int j=0; j<6; j++) 00164 prior_Cov(i+1,j+1) = pose.pose.covariance[6*i+j]; 00165 00166 // make sure we don't leak 00167 if (filter_) delete filter_; 00168 if (prior_) delete prior_; 00169 prior_ = new Gaussian(prior_Mu,prior_Cov); 00170 filter_ = new ExtendedKalmanFilter(prior_); 00171 filter_time_ = pose.header.stamp; 00172 initialized_ = true; 00173 } 00174 00175 00176 00177 void DetectorFilter::decomposeTransform(const geometry_msgs::PoseWithCovarianceStamped& pose, 00178 MatrixWrapper::ColumnVector& vector) 00179 { 00180 assert(vector.rows() == 6); 00181 00182 // construct kdl rotation from quaternion, and extract RPY 00183 KDL::Rotation rot = KDL::Rotation::Quaternion(pose.pose.pose.orientation.x, 00184 pose.pose.pose.orientation.y, 00185 pose.pose.pose.orientation.z, 00186 pose.pose.pose.orientation.w); 00187 rot.GetRPY(vector(4), vector(5), vector(6)); 00188 00189 vector(1) = pose.pose.pose.position.x; 00190 vector(2) = pose.pose.pose.position.y; 00191 vector(3) = pose.pose.pose.position.z; 00192 }; 00193 00194 void DetectorFilter::composeTransform(const MatrixWrapper::ColumnVector& vector, 00195 geometry_msgs::PoseWithCovarianceStamped& pose) 00196 { 00197 assert(vector.rows() == 6); 00198 00199 // construct kdl rotation from vector (x, y, z, Rx, Ry, Rz), and extract quaternion 00200 KDL::Rotation rot = KDL::Rotation::RPY(vector(4), vector(5), vector(6)); 00201 rot.GetQuaternion(pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, 00202 pose.pose.pose.orientation.z, pose.pose.pose.orientation.w); 00203 00204 pose.pose.pose.position.x = vector(1); 00205 pose.pose.pose.position.y = vector(2); 00206 pose.pose.pose.position.z = vector(3); 00207 }; 00208 00209 00210 bool DetectorFilter::getPose(geometry_msgs::PoseWithCovarianceStamped& pose) 00211 { 00212 // only get pose when we received a pose at the input side 00213 if (!initialized_) return false; 00214 00215 // mean 00216 ColumnVector mean = filter_->PostGet()->ExpectedValueGet(); 00217 composeTransform(mean, pose); 00218 00219 // header 00220 pose.header.stamp = filter_time_; 00221 pose.header.frame_id = fixed_frame_; 00222 00223 // covariance 00224 SymmetricMatrix covar = filter_->PostGet()->CovarianceGet(); 00225 for (unsigned int i=0; i<6; i++) 00226 for (unsigned int j=0; j<6; j++) 00227 pose.pose.covariance[6*i+j] = covar(i+1,j+1); 00228 00229 return true; 00230 } 00231 00232 bool DetectorFilter::resetState(std_srvs::Empty::Request &req, 00233 std_srvs::Empty::Response &res ) 00234 { 00235 initialized_ = false; 00236 return true; 00237 } 00238 00239 00240 00241 }// namespace 00242 00243 00244 int main(int argc, char** argv) 00245 { 00246 ros::init(argc, argv, "detector_filter"); 00247 00248 ros::NodeHandle node; 00249 ros::Publisher pose_pub = node.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose_out", 10); 00250 geometry_msgs::PoseWithCovarianceStamped pose; 00251 detector::DetectorFilter detector_filter; 00252 00253 ros::Rate rate(100.0); 00254 while (ros::ok()){ 00255 if (detector_filter.getPose(pose)){ 00256 pose.header.stamp = ros::Time::now(); 00257 pose_pub.publish(pose); 00258 } 00259 ros::spinOnce(); 00260 rate.sleep(); 00261 } 00262 00263 return 0; 00264 } 00265 00266 00267