Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
mobile_robotics
ADC
ADC_2021
iri_adc_landmarks_slam_solver
Commits
dffea66a
Commit
dffea66a
authored
Oct 01, 2021
by
Alejandro Lopez Gestoso
Browse files
Adapted to load form xodr file
parent
6cf69679
Changes
11
Hide whitespace changes
Inline
Side-by-side
cfg/AdcLandmarksSlamSolver.cfg
View file @
dffea66a
...
...
@@ -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
)
...
...
config/amcl_mapping.yaml
View file @
dffea66a
...
...
@@ -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
...
...
config/landmarks_calibration.yaml
View file @
dffea66a
...
...
@@ -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
...
...
config/landmarks_calibration_amcl.yaml
View file @
dffea66a
...
...
@@ -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
...
...
config/localization.yaml
View file @
dffea66a
...
...
@@ -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
...
...
config/slam_mapping.yaml
View file @
dffea66a
...
...
@@ -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
...
...
include/adc_landmarks_slam_solver_alg.h
View file @
dffea66a
...
...
@@ -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.
...
...
launch/node.launch
View file @
dffea66a
...
...
@@ -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)"
/>
...
...
launch/test.launch
View file @
dffea66a
...
...
@@ -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)"
/>
...
...
src/adc_landmarks_slam_solver_alg.cpp
View file @
dffea66a
...
...
@@ -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
;
...
...
src/adc_landmarks_slam_solver_alg_node.cpp
View file @
dffea66a
...
...
@@ -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);
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment