Added inti_pose parameters

cfg/AdcLandmarksSlamSolver.cfg

@@ -55,6 +55,7 @@ gen.add("publish_pose_angle",                 double_t,     0,           "Angle
 gen.add("write_output_files",                 bool_t,       0,           "Boolean to print landmarks info to a txt file",                             False)
 gen.add("output_files_folder",                str_t,        0,           "Output txt file path",                                                      "./")
 gen.add("err_msg_rate",                       double_t,     0,           "Rate to publish err messages",                                              0.5, 0.1, 1.0)
+gen.add("init_pose_covariance",               double_t,     0,           "Initial pose covariance",                                                   0.25, 0.1, 1.0)
 
 landmarks.add("landmarks_candidates_filter_en",   bool_t,       0,           "Boolean to filter landmarks checking if it's a feature on the followings scans before adding it as landmark",   False)
 landmarks.add("landmarks_filter_orientation_en",  bool_t,       0,           "Boolean to add orientation filter to time persistance filter",                                                  False)

launch/node.launch

@@ -6,11 +6,17 @@
   <arg name="launch_prefix"        default=""/>
   <arg name="config_file"          default="$(find iri_adc_landmarks_slam_solver)/config/params.yaml"/>
   <arg name="landmarks_map_file"   default="$(find iri_adc_landmarks_slam_solver)/data/landmarks.txt"/>
+
+  <arg name="initial_pose_x"       default="0.0"/>
+  <arg name="initial_pose_y"       default="0.0"/>
+  <arg name="initial_pose_yaw"     default="0.0"/>
+
   <!-- <arg name="topic_name"  default="new_topic_name"/> -->
   <arg name="estimated_pose_topic_name" default="~/estimated_pose"/>
   <arg name="initialpose_topic_name" default="~/initialpose"/>
   <arg name="features_topic_name" default="~/features"/>
 
+
   <node name="$(arg node_name)"
         pkg ="iri_adc_landmarks_slam_solver"
         type="iri_adc_landmarks_slam_solver"
@@ -18,6 +24,9 @@
         launch-prefix="$(arg launch_prefix)">
     <rosparam file="$(arg config_file)" command="load"/>
     <param name="landmarks_map_file" value="$(arg landmarks_map_file)"/>
+    <param name="initial_pose_x"     value="$(arg initial_pose_x)" />
+    <param name="initial_pose_y"     value="$(arg initial_pose_y)" />
+    <param name="initial_pose_yaw"     value="$(arg initial_pose_yaw)" />
     <!--<remap from="~/topic" to="$(arg topic_name)"/>-->
     <remap from="~/estimated_pose" to="$(arg estimated_pose_topic_name)"/>
     <remap from="~/initialpose" to="$(arg initialpose_topic_name)"/>

src/adc_landmarks_slam_solver_alg_node.cpp

@@ -80,6 +80,42 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
     }
     load_landmarks();
   }
+  if (!this->config_.amcl_localization)
+  {
+    this->new_initialpose_ = true;
+    if(!this->private_node_handle_.getParam("initial_pose_x", this->estimated_pose_(0)))
+    {
+      ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'initial_pose_x' not found.");
+      this->new_initialpose_ = false;
+    }
+    if(!this->private_node_handle_.getParam("initial_pose_y", this->estimated_pose_(1)))
+    {
+      ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'initial_pose_y' not found.");
+      this->new_initialpose_ = false;
+    }
+    if(!this->private_node_handle_.getParam("initial_pose_yaw", this->estimated_pose_(2)))
+    {
+      ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'initial_pose_yaw' not found.");
+      this->new_initialpose_ = false;
+    }
+    if (new_initialpose_)
+    {
+      this->init_tf_map_odom_ = true;
+      if(!this->private_node_handle_.getParam("init_pose_covariance", this->estimated_sigma_(0)))
+      {
+        ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'init_pose_covariance' not found. Setting it to 0.25");
+        this->estimated_sigma_(0) = 0.25;
+      }
+      this->estimated_sigma_(1) =this->estimated_sigma_(0);
+      this->estimated_sigma_(2) =this->estimated_sigma_(0);
+    }
+  }
+  else
+  {
+    this->init_tf_map_odom_ = this->config_.amcl_localization;
+    this->new_initialpose_ = false;
+  }
+  
 
   // [init publishers]
   this->frame_data_publisher_ = this->private_node_handle_.advertise<visualization_msgs::MarkerArray>("frame_data", 1);
@@ -100,10 +136,8 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
   this->tf_odom_base_ << 0.0, 0.0, 0.0;
   this->new_features_data_ = false;
   this->new_estimated_pose_ = false;
-  this->new_initialpose_ = false;
   this->reset_estimated_pose_ = false;
 
-  this->init_tf_map_odom_ = this->config_.amcl_localization;
   this->last_update_t_ = ros::Time::now();
   this->last_update_odom_base_ << 0.0, 0.0, 0.0;
   this->waiting_for_feature_ = false;