Merge pull request #221 from UbiquityRobotics/bugfix-#220

Allow covariance to be overridden by launch file
UbiquityRobotics · Jun 11, 2020 · ff8a356 · ff8a356
2 parents b6d1aa2 + afa6c69
commit ff8a356
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 3 deletions.
diff --git a/fiducial_slam/launch/fiducial_slam.launch b/fiducial_slam/launch/fiducial_slam.launch
@@ -13,7 +13,7 @@
   <arg name="fiducial_len" default="0.14"/>
   <arg name="systematic_error" default="0.01"/>
   <arg name="do_pose_estimation" default="false"/>
-  <arg name="covariance_diagonal" default="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]"/>
+  <arg name="covariance_diagonal" default=""/>
 
   <node type="fiducial_slam" pkg="fiducial_slam" output="screen" 
     name="fiducial_slam">
@@ -28,7 +28,7 @@
     <param name="do_pose_estimation" value="$(arg do_pose_estimation)"/>
     <param name="sum_error_in_quadrature" value="true"/>
     <param name="fiducial_len" value="$(arg fiducial_len)"/>
-    <param name="covariance_diagonal" value="$(arg covariance_diagonal)"/>
+    <rosparam param="covariance_diagonal" subst_value="True">$(arg covariance_diagonal)</rosparam>
     <remap from="/camera_info" to="$(arg camera)/camera_info"/>
 
   </node>

diff --git a/fiducial_slam/src/map.cpp b/fiducial_slam/src/map.cpp
@@ -110,9 +110,13 @@ Map::Map(ros::NodeHandle &nh) : tfBuffer(ros::Duration(30.0)) {
     std::fill(covarianceDiagonal.begin(), covarianceDiagonal.end(), 0);
     overridePublishedCovariance = nh.getParam("covariance_diagonal", covarianceDiagonal);
     if (overridePublishedCovariance) {
+        if (covarianceDiagonal.size() != 6) {
+            ROS_WARN("ignoring covariance_diagonal because it has %ld elements, not 6", covarianceDiagonal.size());
+            overridePublishedCovariance = false;
+        }
         // Check to make sure that the diagonal is non-zero
         for (auto variance : covarianceDiagonal) {
-            if (variance = 0) {
+            if (variance == 0) {
                 ROS_WARN("ignoring covariance_diagonal because it has 0 values");
                 std::fill(covarianceDiagonal.begin(), covarianceDiagonal.end(), 0);
                 break;