Merge pull request #508 from CMU-Robotics-Club/abhinav_localizer_cals

Abhinav localizer cals
CMU-Robotics-Club · Jan 25, 2019 · 7b93ea6 · 7b93ea6
2 parents 8e13abb + 110ca58
commit 7b93ea6
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 0 deletions.
diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/launch/init_localizer_params.launch b/Software/real_time/ROS_RoboBuggy/src/robobuggy/launch/init_localizer_params.launch
@@ -0,0 +1,6 @@
+<launch>
+    <node name="Transistor_Localizer" pkg="robobuggy" type="Transistor_Localizer" required="true" output="screen">
+        <rosparam param="init_r_diagonal">[2.0, 2.0, 2.0, 2.00, 2.00]</rosparam>
+        <rosparam param="init_p_diagonal">[25.0, 25.0, 25.0, 25.0, 25.0]</rosparam>
+    </node>
+</launch>
diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/src/transistor/localizer/Localizer.cpp b/Software/real_time/ROS_RoboBuggy/src/robobuggy/src/transistor/localizer/Localizer.cpp
@@ -123,6 +123,19 @@ void Localizer::init_R()
     ;
 
     std::stringstream s;
+    std::vector<double> init_r_diagonal;    
+
+    if(!nh.getParam(NODE_NAME + "/init_r_diagonal", init_r_diagonal)){
+        ROS_INFO_STREAM("R Matrix Diagonal Parameter not found, using default");
+        init_r_diagonal = {2.0, 2.0, 2.0, 2.0, 2.0};
+    }
+
+    for (int i = 0; i < init_r_diagonal.size(); i++)
+    {
+        R(i,i) = init_r_diagonal[i];
+    }
+
+
     s << R << std::endl;
 
     ROS_INFO("Initialized R Matrix to : \n%s", s.str().c_str());
@@ -140,6 +153,18 @@ void Localizer::init_P()
     ;
 
     std::stringstream s;
+    std::vector<double> init_p_diagonal; 
+
+    if(!nh.getParam(NODE_NAME + "/init_p_diagonal",init_p_diagonal)){
+        ROS_INFO_STREAM("P Matrix Diagonal Parameter not found, using default");
+        init_p_diagonal = {25.0, 25.0, 25.0, 25.0, 25.0};
+    }
+
+    for (int i = 0; i < init_p_diagonal.size(); i++)
+    {
+         P(i,i) = init_p_diagonal[i];
+    }
+
     s << P << std::endl;
 
     ROS_INFO("Initialized P Matrix to : \n%s", s.str().c_str());