Skip to content
GitLab
Commit ff37d8d6 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added new features to update a map

parent 7200ee8d
Hide whitespace changes
Inline Side-by-side
include/adc_landmarks_slam_solver_alg.h
View file @ ff37d8d6
......@@ -79,6 +79,7 @@ typedef struct {
bool detected; ///< If it has been detected.
std::string source_frame_id; ///< Frame id of the last sensor that detect it.
int type; ///< Feature type.
bool fixed; ///< If its position is fixed
}LandmarkInfo;
/**
......
src/adc_landmarks_slam_solver_alg.cpp
View file @ ff37d8d6
......@@ -38,6 +38,12 @@ void AdcLandmarksSlamSolverAlgorithm::config_update(Config& config, uint32_t lev
{
this->lock();
if (config.landmarks_pos_fixed != this->config_.landmarks_pos_fixed)
{
for (std::map<double, LandmarkInfo>::iterator it = this->landmarks_.begin(); it != this->landmarks_.end(); ++it)
it->second.fixed = config.landmarks_pos_fixed;
}
// save the current configuration
this->config_=config;
......@@ -65,6 +71,8 @@ bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmar
l.pose(2) = l3;
l.type = t;
l.source_frame_id = "";
l.fixed = this->config_.landmarks_pos_fixed;
std::cout << (this->config_.landmarks_pos_fixed? "fixed true": "Fixed false");
this->landmarks_[id] = l;
}
// std::cout << "landmarks size = " << (int) this->global_landmarks.size() << std::endl;
......@@ -268,10 +276,8 @@ void AdcLandmarksSlamSolverAlgorithm::add_newest_frame_to_ceres(bool _estimated_
ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<LandmarkResidualFromBase2D, 2, 3, 2>(new LandmarkResidualFromBase2D(land_res_it->r, land_res_it->theta, this->config_.sensor_sigma_r, this->config_.sensor_sigma_th));
this->problem_.AddResidualBlock(cost_function, new ceres::CauchyLoss(1.0), this->frames_data_.back().robot_state.data(), land_it->second.pose.data());
if(this->config_.landmarks_pos_fixed)
{
if(land_it->second.fixed)
this->problem_.SetParameterBlockConstant(land_it->second.pose.data());
}
}
else
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgorithm::add_newest_frame_to_ceres -> No landmark has been found with id " << land_res_it->landmark_key);
......
src/adc_landmarks_slam_solver_alg_node.cpp
View file @ ff37d8d6
......@@ -16,56 +16,56 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
if(!this->private_node_handle_.getParam("global_frame", this->config_.global_frame))
{
ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'global_frame' not found. Setting it to map.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'global_frame' not found. Setting it to map.");
this->config_.global_frame = "map";
}
if(!this->private_node_handle_.getParam("odom_frame", this->config_.odom_frame))
{
ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'odom_frame' not found. Setting it to odom.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'odom_frame' not found. Setting it to odom.");
this->config_.odom_frame = "odom";
}
if(!this->private_node_handle_.getParam("base_link_frame", this->config_.base_link_frame))
{
ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'base_link_frame' not found. Setting it to base_link.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'base_link_frame' not found. Setting it to base_link.");
this->config_.base_link_frame = "base_link";
}
//Get flags
if(!this->private_node_handle_.getParam("load_landmarks", this->config_.load_landmarks))
{
ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'load_landmarks' not found. Setting it to false.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'load_landmarks' not found. Setting it to false.");
this->config_.load_landmarks = false;
}
if(!this->private_node_handle_.getParam("landmarks_pos_fixed", this->config_.landmarks_pos_fixed))
{
ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'landmarks_pos_fixed' not found. Setting it to false.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'landmarks_pos_fixed' not found. Setting it to false.");
this->config_.landmarks_pos_fixed = false;
}
if(!this->private_node_handle_.getParam("search_for_new_landmarks", this->config_.search_for_new_landmarks))
{
ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'search_for_new_landmarks' not found. Setting it to false.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'search_for_new_landmarks' not found. Setting it to false.");
this->config_.search_for_new_landmarks = false;
}
if(!this->private_node_handle_.getParam("amcl_localization", this->config_.amcl_localization))
{
ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'amcl_localization' not found. Setting it to false.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'amcl_localization' not found. Setting it to false.");
this->config_.amcl_localization = false;
}
if(!this->private_node_handle_.getParam("calculate_covariance", this->config_.calculate_covariance))
{
ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'calculate_covariance' not found. Setting it to false.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'calculate_covariance' not found. Setting it to false.");
this->config_.calculate_covariance = false;
}
if(!this->private_node_handle_.getParam("publish_tf_map_odom", this->config_.publish_tf_map_odom))
{
ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'publish_tf_map_odom' not found. Setting it to false.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'publish_tf_map_odom' not found. Setting it to false.");
this->config_.publish_tf_map_odom = false;
}
......@@ -75,7 +75,7 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
{
if(!this->private_node_handle_.getParam("landmarks_map_file", this->config_.landmarks_map_file))
{
ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'landmarks_map_file' not found. Setting it to \"landmarks.txt\".");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'landmarks_map_file' not found. Setting it to \"landmarks.txt\".");
this->config_.landmarks_map_file = "landmarks.txt";
}
load_landmarks();
......@@ -85,17 +85,17 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
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.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: 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.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: 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.");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'initial_pose_yaw' not found.");
this->new_initialpose_ = false;
}
if (new_initialpose_)
......@@ -103,7 +103,7 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
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");
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: 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);
......@@ -226,7 +226,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
this->alg_.remove_all_frames_from_ceres();
//Remove landmarks if necessary
if (!this->config_.load_landmarks)
if (this->config_.search_for_new_landmarks)
{
if (this->landmarks_MarkerArray_msg_.markers.size() > 0)
{
......@@ -234,7 +234,10 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
this->landmarks_MarkerArray_msg_.markers[0].action = visualization_msgs::Marker::DELETEALL;
this->landmarks_publisher_.publish(this->landmarks_MarkerArray_msg_);
}
this->alg_.delete_landmarks();
if (this->config_.load_landmarks)
load_landmarks();
else
this->alg_.delete_landmarks();
}
//Remove candidates
......@@ -571,7 +574,7 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
geometry_msgs::TransformStamped tf_odom_sensor_msg;
if (!this->tf2_buffer.canTransform(this->config_.odom_frame, tf_map_sensor_msg.child_frame_id, tf_map_sensor_msg.header.stamp, ros::Duration(this->config_.tf_timeout)))
{
ROS_WARN_STREAM("LandmarksTrackerAlgNode:check_landmarks -> Can't transform from " << this->config_.odom_frame << " to " << tf_map_sensor_msg.child_frame_id << " at " << tf_map_sensor_msg.header.stamp);
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:check_landmarks -> Can't transform from " << this->config_.odom_frame << " to " << tf_map_sensor_msg.child_frame_id << " at " << tf_map_sensor_msg.header.stamp);
return false;
}
tf_odom_sensor_msg = this->tf2_buffer.lookupTransform(this->config_.odom_frame, tf_map_sensor_msg.child_frame_id, tf_map_sensor_msg.header.stamp);
......@@ -629,6 +632,7 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
landmark.detected = true;
landmark.source_frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
landmark.type = (_front? this->front_features_: this->rear_features_)[i].type;
landmark.fixed = false;
double landmark_key = std::fmod(this->front_features_stamp_.toSec(), L_KEY_DIV)*100+i;
(_front? this->front_features_: this->rear_features_)[i].landmark_key = landmark_key;
this->alg_.add_landmark(landmark_key, landmark);
......@@ -1199,7 +1203,7 @@ void AdcLandmarksSlamSolverAlgNode::node_config_update(Config &config, uint32_t
{
std::string file_name;
file_name = this->config_.output_files_folder + "landmarks.txt";
ROS_ERROR_STREAM("LandmarksTrackerAlgNode:: Can't open " << file_name << " file.");
ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgNode:: Can't open " << file_name << " file.");
}
}
this->config_=config;
......
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