diff --git a/cfg/AdcLandmarksSlamSolver.cfg b/cfg/AdcLandmarksSlamSolver.cfg
index 736c419987526aed833d7eddc68687f8a8f4913c..2f9bcab69558ec4d33f067e25e434062fb37f4bf 100755
--- a/cfg/AdcLandmarksSlamSolver.cfg
+++ b/cfg/AdcLandmarksSlamSolver.cfg
@@ -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)
diff --git a/config/amcl_mapping.yaml b/config/amcl_mapping.yaml
index a6f5f655048a8b6001539c8dacd07d949f833b7e..f8dc941f26d6880f751eda3e18c0d87e5fdb82db 100644
--- a/config/amcl_mapping.yaml
+++ b/config/amcl_mapping.yaml
@@ -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
diff --git a/config/landmarks_calibration.yaml b/config/landmarks_calibration.yaml
index 869beea07c900aa8de069881695ab811bd542420..7a1d39f4a6b96f3ae7f65281899765bbf88ef692 100644
--- a/config/landmarks_calibration.yaml
+++ b/config/landmarks_calibration.yaml
@@ -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
diff --git a/config/landmarks_calibration_amcl.yaml b/config/landmarks_calibration_amcl.yaml
index a49cdf3f7bf0d2c69d2150ca9dac506292b34fa3..3f70650c7d42bd4f11572c5c95f1c51afab7b643 100644
--- a/config/landmarks_calibration_amcl.yaml
+++ b/config/landmarks_calibration_amcl.yaml
@@ -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
diff --git a/config/localization.yaml b/config/localization.yaml
index b1c10392788aefb473cfbeeebcf9a419b595d734..3b1d6513d51542da662fda14c885dbf560905a8d 100644
--- a/config/localization.yaml
+++ b/config/localization.yaml
@@ -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
diff --git a/config/slam_mapping.yaml b/config/slam_mapping.yaml
index 1442dc3762c9548de90921cb1613d4214c368c68..052370fd079ced7e0425c64a3fa26e82894d9359 100644
--- a/config/slam_mapping.yaml
+++ b/config/slam_mapping.yaml
@@ -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
diff --git a/include/adc_landmarks_slam_solver_alg.h b/include/adc_landmarks_slam_solver_alg.h
index f47f01532f1a98024aef06a9d3bf3b19a57d121f..44bfa155397effde23e3679df92452200184aef1 100644
--- a/include/adc_landmarks_slam_solver_alg.h
+++ b/include/adc_landmarks_slam_solver_alg.h
@@ -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.
diff --git a/launch/node.launch b/launch/node.launch
index 336600572dd4854dba192f093306d402cd96fd4f..bf95016a32102e874a43afc8fa380876bb04443c 100644
--- a/launch/node.launch
+++ b/launch/node.launch
@@ -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)" />
diff --git a/launch/test.launch b/launch/test.launch
index 9a90334e7e2aac73c09ae0773c26ae6d642958ef..27a573f8d1ad22be27cc83adf9d43eb18acf4d5c 100644
--- a/launch/test.launch
+++ b/launch/test.launch
@@ -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)"/>
diff --git a/src/adc_landmarks_slam_solver_alg.cpp b/src/adc_landmarks_slam_solver_alg.cpp
index 3a27d33517788e9864bb820e9f011005a16c87b5..be73746c1eae6f8c66bd63646087b2dc77263da1 100644
--- a/src/adc_landmarks_slam_solver_alg.cpp
+++ b/src/adc_landmarks_slam_solver_alg.cpp
@@ -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;
diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp
index 5a6d4f4da9a77fba68e435c6dd32fba57fdc6381..2dd6158a42943c4120b14a1428dbe553dc550b6c 100644
--- a/src/adc_landmarks_slam_solver_alg_node.cpp
+++ b/src/adc_landmarks_slam_solver_alg_node.cpp
@@ -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);