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

Adapted to load form xodr file

parent 6cf69679
......@@ -71,6 +71,7 @@ ceres.add("update_problem_features_detected", int_t, 0, "Up
ceres.add("wait_feature_detected_timeout", double_t, 0, "Timeout on a update problem event to wait for a feature detection", 0.2, 0.01, 0.5)
ceres.add("landmarks_map_file", str_t, 0, "Input txt file path", "landmarks.txt")
ceres.add("load_landmarks_from_xodr", bool_t, 0, "If the landmarks map file is an xodr", False)
ceres.add("landmarks_xodr_scale", double_t, 0, "Xodr scale", 1.0, 0.0)
ceres.add("problem_frame_window", int_t, 0, "Max number of frames to add", 30, -1, 500)
flags.add("publish_visualization", bool_t, 0, "Boolean to publish visualization markers", False)
......
......@@ -32,6 +32,7 @@ landmark_mahalanobis_dist: 3.5
landmarks_min_detections: 4
landmarks_filter_orientation_en: True
landmarks_orientation_th: 0.4
landmarks_xodr_scale: 10.0
sensor_sigma_r: 0.2
sensor_sigma_th: 0.3
......
......@@ -32,6 +32,7 @@ landmark_mahalanobis_dist: 3.5
landmarks_min_detections: 4
landmarks_filter_orientation_en: True
landmarks_orientation_th: 0.4
landmarks_xodr_scale: 10.0
sensor_sigma_r: 0.2
sensor_sigma_th: 0.3
......
......@@ -32,6 +32,7 @@ landmark_mahalanobis_dist: 3.5
landmarks_min_detections: 4
landmarks_filter_orientation_en: True
landmarks_orientation_th: 0.4
landmarks_xodr_scale: 10.0
sensor_sigma_r: 0.2
sensor_sigma_th: 0.3
......
......@@ -32,6 +32,7 @@ landmark_mahalanobis_dist: 3.5
landmarks_min_detections: 4
landmarks_filter_orientation_en: True
landmarks_orientation_th: 0.4
landmarks_xodr_scale: 10.0
sensor_sigma_r: 0.2
sensor_sigma_th: 0.3
......
......@@ -32,6 +32,7 @@ landmark_mahalanobis_dist: 3.5
landmarks_min_detections: 4
landmarks_filter_orientation_en: True
landmarks_orientation_th: 0.4
landmarks_xodr_scale: 10.0
sensor_sigma_r: 0.2
sensor_sigma_th: 0.3
......
......@@ -52,6 +52,7 @@
#define LOC_SIGN_ID_OFFSET 100
#define LOC_SIGN_TRIANGLE_WIDTH 0.074
#define LOC_SIGN_TRIANGLE_ANGLE M_PI/2
#define LOC_SIGN_TRIANGLE_ANGLE_OFFSET M_PI/2
//include adc_landmarks_slam_solver_alg main library
......@@ -131,10 +132,12 @@ class AdcLandmarksSlamSolverAlgorithm
* \param _fixed If landmarks are fixed.
*
* \param _from_xodr If landamrks are loaded form an xodr file.
*
* \param _scale Xodr scale.
*
* \return If success.
*/
bool load_landmarks(const std::string& _landmarks_file, bool _fixed, bool _from_xodr);
bool load_landmarks(const std::string& _landmarks_file, bool _fixed, bool _from_xodr, double _scale);
/**
* \brief Function to load landmarks map from a xodr file.
......@@ -142,10 +145,12 @@ class AdcLandmarksSlamSolverAlgorithm
* \param _landmarks_file The route to the landmarks file to be loaded.
*
* \param _fixed If landmarks are fixed.
*
* \param _scale Xodr scale.
*
* \return If success.
*/
bool load_from_xodr(const std::string& _landmarks_file, bool _fixed);
bool load_from_xodr(const std::string& _landmarks_file, bool _fixed, double _scale);
/**
* \brief Function to add a landmark.
......
......@@ -6,6 +6,7 @@
<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="load_landmarks_from_xodr" default="false"/>
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
......@@ -25,6 +26,7 @@
launch-prefix="$(arg launch_prefix)">
<rosparam file="$(arg config_file)" command="load"/>
<param name="landmarks_map_file" value="$(arg landmarks_map_file)"/>
<param name="load_landmarks_from_xodr" value="$(arg load_landmarks_from_xodr)"/>
<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)" />
......
......@@ -9,6 +9,7 @@
<arg name="config_file" default="amcl_mapping.yaml"/>
<arg name="data_dir" default="$(find iri_adc_landmarks_slam_solver)/data/"/>
<arg name="landmarks_file" default="landmarks.txt"/>
<arg name="load_landmarks_from_xodr" default="false"/>
<arg name="estimated_pose_topic_name" default="/$(arg ns)/estimated_pose"/>
<arg name="initialpose_topic_name" default="/$(arg ns)/initialpose"/>
......@@ -21,6 +22,7 @@
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="config_file" value="$(arg config_dir)/$(arg config_file)"/>
<arg name="landmarks_map_file" value="$(arg data_dir)/$(arg landmarks_file)"/>
<arg name="load_landmarks_from_xodr" value="$(arg load_landmarks_from_xodr)"/>
<arg name="estimated_pose_topic_name" value="$(arg estimated_pose_topic_name)"/>
<arg name="initialpose_topic_name" value="$(arg initialpose_topic_name)"/>
<arg name="features_topic_name" value="$(arg features_topic_name)"/>
......
......@@ -55,10 +55,10 @@ void AdcLandmarksSlamSolverAlgorithm::config_update(Config& config, uint32_t lev
}
// AdcLandmarksSlamSolverAlgorithm Public API
bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmarks_file, bool _fixed, bool _from_xodr)
bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmarks_file, bool _fixed, bool _from_xodr, double _scale)
{
if (_from_xodr)
return load_from_xodr(_landmarks_file, _fixed);
return load_from_xodr(_landmarks_file, _fixed, _scale);
std::ifstream landmarks_file;
landmarks_file.open(_landmarks_file.c_str());
if (!landmarks_file.good())
......@@ -85,12 +85,13 @@ bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmar
return true;
}
bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmarks_file, bool _fixed)
bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmarks_file, bool _fixed, double _scale)
{
COpendriveRoad road;
std::string type, subtype;
TOpendriveWorldPose world;
try{
road.set_scale_factor(_scale);
road.load(_landmarks_file);
std::map<double, LandmarkInfo>().swap(this->landmarks_);
......@@ -118,7 +119,7 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
loc_yaw = M_PI + LOC_SIGN_TRIANGLE_ANGLE/2;
l.pose(0) = world.x + loc_x*std::cos(world.heading) - loc_y*std::sin(world.heading);
l.pose(1) = world.y + loc_x*std::sin(world.heading) + loc_y*std::cos(world.heading);
l.pose(2) = loc_yaw + world.heading;
l.pose(2) = loc_yaw + world.heading + LOC_SIGN_TRIANGLE_ANGLE_OFFSET;
id = LOC_SIGN_ID_OFFSET + (sign.get_id()%LOC_SIGN_ID_OFFSET)*2-1;
this->landmarks_[id] = l;
......@@ -128,7 +129,7 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
loc_yaw = -LOC_SIGN_TRIANGLE_ANGLE/2;
l.pose(0) = world.x + loc_x*std::cos(world.heading) - loc_y*std::sin(world.heading);
l.pose(1) = world.y + loc_x*std::sin(world.heading) + loc_y*std::cos(world.heading);
l.pose(2) = loc_yaw + world.heading;
l.pose(2) = loc_yaw + world.heading + LOC_SIGN_TRIANGLE_ANGLE_OFFSET;
id = LOC_SIGN_ID_OFFSET + (sign.get_id()%LOC_SIGN_ID_OFFSET)*2;
this->landmarks_[id] = l;
......
......@@ -84,6 +84,15 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'load_landmarks_from_xodr' not found. Setting it to false.");
this->config_.load_landmarks_from_xodr = false;
}
if (this->config_.load_landmarks_from_xodr)
{
if(!this->private_node_handle_.getParam("landmarks_xodr_scale", this->config_.landmarks_xodr_scale))
{
ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'landmarks_xodr_scale' not found. Setting it to false.");
this->config_.landmarks_xodr_scale = false;
}
}
load_landmarks();
}
if (!this->config_.amcl_localization)
......@@ -901,7 +910,7 @@ bool AdcLandmarksSlamSolverAlgNode::get_tf_map_odom(ros::Time _t, Eigen::Vector3
void AdcLandmarksSlamSolverAlgNode::load_landmarks(void)
{
ROS_INFO_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> Loading from file " << this->config_.landmarks_map_file);
if (!this->alg_.load_landmarks(this->config_.landmarks_map_file, this->config_.landmarks_pos_fixed, this->config_.load_landmarks_from_xodr))
if (!this->alg_.load_landmarks(this->config_.landmarks_map_file, this->config_.landmarks_pos_fixed, this->config_.load_landmarks_from_xodr, this->config_.landmarks_xodr_scale))
ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> File" << this->config_.landmarks_map_file << " can't be opened");
}
......@@ -948,7 +957,7 @@ void AdcLandmarksSlamSolverAlgNode::publish_landmarks_markers()
this->landmarks_MarkerArray_msg_.markers[i].id = i;
this->landmarks_MarkerArray_msg_.markers[i].ns = "landmarks";
// this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::ARROW;
this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::CUBE;
this->landmarks_MarkerArray_msg_.markers[i].action = visualization_msgs::Marker::ADD;
// this->landmarks_MarkerArray_msg_.markers[i].lifetime = ros::Duration(0.1);
......
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