Commit b962b289 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added inti_pose parameters

parent 3a29e29a
...@@ -55,6 +55,7 @@ gen.add("publish_pose_angle", double_t, 0, "Angle ...@@ -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("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("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("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_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) landmarks.add("landmarks_filter_orientation_en", bool_t, 0, "Boolean to add orientation filter to time persistance filter", False)
......
...@@ -6,11 +6,17 @@ ...@@ -6,11 +6,17 @@
<arg name="launch_prefix" default=""/> <arg name="launch_prefix" default=""/>
<arg name="config_file" default="$(find iri_adc_landmarks_slam_solver)/config/params.yaml"/> <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="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="topic_name" default="new_topic_name"/> -->
<arg name="estimated_pose_topic_name" default="~/estimated_pose"/> <arg name="estimated_pose_topic_name" default="~/estimated_pose"/>
<arg name="initialpose_topic_name" default="~/initialpose"/> <arg name="initialpose_topic_name" default="~/initialpose"/>
<arg name="features_topic_name" default="~/features"/> <arg name="features_topic_name" default="~/features"/>
<node name="$(arg node_name)" <node name="$(arg node_name)"
pkg ="iri_adc_landmarks_slam_solver" pkg ="iri_adc_landmarks_slam_solver"
type="iri_adc_landmarks_slam_solver" type="iri_adc_landmarks_slam_solver"
...@@ -18,6 +24,9 @@ ...@@ -18,6 +24,9 @@
launch-prefix="$(arg launch_prefix)"> launch-prefix="$(arg launch_prefix)">
<rosparam file="$(arg config_file)" command="load"/> <rosparam file="$(arg config_file)" command="load"/>
<param name="landmarks_map_file" value="$(arg landmarks_map_file)"/> <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="~/topic" to="$(arg topic_name)"/>-->
<remap from="~/estimated_pose" to="$(arg estimated_pose_topic_name)"/> <remap from="~/estimated_pose" to="$(arg estimated_pose_topic_name)"/>
<remap from="~/initialpose" to="$(arg initialpose_topic_name)"/> <remap from="~/initialpose" to="$(arg initialpose_topic_name)"/>
......
...@@ -80,6 +80,42 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) : ...@@ -80,6 +80,42 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
} }
load_landmarks(); 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] // [init publishers]
this->frame_data_publisher_ = this->private_node_handle_.advertise<visualization_msgs::MarkerArray>("frame_data", 1); this->frame_data_publisher_ = this->private_node_handle_.advertise<visualization_msgs::MarkerArray>("frame_data", 1);
...@@ -100,10 +136,8 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) : ...@@ -100,10 +136,8 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
this->tf_odom_base_ << 0.0, 0.0, 0.0; this->tf_odom_base_ << 0.0, 0.0, 0.0;
this->new_features_data_ = false; this->new_features_data_ = false;
this->new_estimated_pose_ = false; this->new_estimated_pose_ = false;
this->new_initialpose_ = false;
this->reset_estimated_pose_ = 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_t_ = ros::Time::now();
this->last_update_odom_base_ << 0.0, 0.0, 0.0; this->last_update_odom_base_ << 0.0, 0.0, 0.0;
this->waiting_for_feature_ = false; this->waiting_for_feature_ = false;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment