From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Fri, 4 Oct 2019 21:49:13 +0000 (-0300)
Subject: Fix retrieve of ROS parameters.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=e44521295aabaaa9474612f7d8eed3d3361701ec;p=imufusion_ros.git

Fix retrieve of ROS parameters.
---

diff --git a/src/ekf.cpp b/src/ekf.cpp
index 80490ab..62d21ba 100644
--- a/src/ekf.cpp
+++ b/src/ekf.cpp
@@ -75,18 +75,18 @@ EkfNode::EkfNode(const ros::NodeHandle &node,const char *frameId)
 	Eigen::MatrixXd P0(7,7);
 	P0.setIdentity();
 	std::vector<double> P0Diag;
-	if(node_.getParam("P",P0Diag))
+	ros::NodeHandle nh("~");
+	if(nh.getParam("P",P0Diag))
 	{
 		Eigen::Map<Eigen::VectorXd> diag(P0Diag.data(),P0.rows());
 		P0=diag.asDiagonal();
 	}
-	else P0*=1e-5;
+	else P0*=1e5;
 
 	Eigen::MatrixXd Pw(7,7);
 	Pw.setIdentity();
 	std::vector<double> PwDiag;
-	ros::NodeHandle nh("~");
-	if(node_.getParam("/ekf/Pw",PwDiag))
+	if(nh.getParam("Pw",PwDiag))
 	{
 		Eigen::Map<Eigen::VectorXd> diag(PwDiag.data(),Pw.rows());
 		Pw=diag.asDiagonal();