add initialization for roll&pitch, remove condition in launch (autowa…

…refoundation#209)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

localization/ekf_localizer/launch/ekf_localizer.launch.xml

@@ -46,15 +46,11 @@
 
   <node pkg="ekf_localizer" exec="ekf_localizer" name="ekf_localizer" output="screen">
 
-    <remap unless="$(var use_pose_with_covariance)" from="in_pose" to="$(var input_pose_name)"/>
-    <remap unless="$(var use_pose_with_covariance)" from="in_pose_with_covariance" to="input_pose_with_cov_UNUSED"/>
-    <remap if="$(var use_pose_with_covariance)" from="in_pose" to="input_pose_UNUSED"/>
-    <remap if="$(var use_pose_with_covariance)" from="in_pose_with_covariance" to="$(var input_pose_with_cov_name)"/>
-
-    <remap unless="$(var use_twist_with_covariance)" from="in_twist" to="$(var input_twist_name)"/>
-    <remap unless="$(var use_twist_with_covariance)" from="in_twist_with_covariance" to="input_twist_with_covariance_UNUSED"/>
-    <remap if="$(var use_twist_with_covariance)" from="in_twist" to="input_twist_UNUSED"/>
-    <remap if="$(var use_twist_with_covariance)" from="in_twist_with_covariance" to="$(var input_twist_with_cov_name)"/>
+    <remap from="in_pose" to="$(var input_pose_name)"/>
+    <remap from="in_pose_with_covariance" to="$(var input_pose_with_cov_name)"/>
+
+    <remap from="in_twist" to="$(var input_twist_name)"/>
+    <remap from="in_twist_with_covariance" to="$(var input_twist_with_cov_name)"/>
 
     <remap from="initialpose" to="$(var input_initial_pose_name)"/>
 

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -233,17 +233,14 @@ void EKFLocalizer::setCurrentResult()
   current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y);
 
   tf2::Quaternion q_tf;
-  double roll = 0.0, pitch = 0.0, yaw = 0.0;
+  double roll = 0.0, pitch = 0.0;
   if (current_pose_ptr_ != nullptr) {
     current_ekf_pose_.pose.position.z = current_pose_ptr_->pose.position.z;
     tf2::fromMsg(current_pose_ptr_->pose.orientation, q_tf); /* use Pose pitch and roll */
-    tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw);
-  } else {
-    // current_ekf_pose_.pose.position.z = 0.0;
-    // roll = 0;
-    // pitch = 0;
+    double yaw_tmp;
+    tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw_tmp);
   }
-  yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB);
+  double yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB);
   current_ekf_pose_.pose.orientation = createQuaternionFromRPY(roll, pitch, yaw);
 
   current_ekf_pose_no_yawbias_ = current_ekf_pose_;