Skip to content
GitLab
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
f5f3d667
Commit
f5f3d667
authored
Aug 05, 2021
by
Alejandro Lopez Gestoso
Browse files
Adapted to two features input topics
parent
a3492b18
Changes
4
Expand all
Hide whitespace changes
Inline
Side-by-side
cfg/AdcLandmarksSlamSolver.cfg
View file @
f5f3d667
...
...
@@ -48,6 +48,7 @@ gen.add("global_frame", str_t, 0, "Global
gen
.
add
(
"odom_frame"
,
str_t
,
0
,
"Odom frame"
,
"odom"
)
gen
.
add
(
"base_link_frame"
,
str_t
,
0
,
"Robot base_link frame"
,
"base_link"
)
gen
.
add
(
"tf_timeout"
,
double_t
,
0
,
"Timeout to find a transform"
,
0.2
,
0.1
,
2.0
)
gen
.
add
(
"old_feature_timeout"
,
double_t
,
0
,
"Timeout to set a features source as old"
,
0.5
,
0.1
,
2.0
)
gen
.
add
(
"amcl_pose_estimated_sigma"
,
double_t
,
0
,
"AMCL pose sigma when using AMCL localization"
,
1
,
0.1
,
10
)
gen
.
add
(
"publish_pose_rate"
,
double_t
,
0
,
"Rate to publish the robot pose"
,
1.0
,
0.00001
,
10
)
gen
.
add
(
"publish_pose_distance"
,
double_t
,
0
,
"Distance from last robot state to publish the robot pose"
,
1.0
,
0.1
,
100000
)
...
...
include/adc_landmarks_slam_solver_alg_node.h
View file @
f5f3d667
...
...
@@ -73,6 +73,7 @@ typedef struct {
double
th
;
///< Angle at detection.
double
landmark_key
;
///< The landmark id detected. -1 if no landmark detected.
int
type
;
///< Feature type.
std
::
string
frame
;
///< features origin frame.
}
FeatureInfo
;
/**
...
...
@@ -89,9 +90,18 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
double
covariance_m_
[
3
*
3
];
///< Last robot pose covariance.
bool
new_features_data_
;
///< Bool to know that new features has been received.
std
::
string
features_frame_
;
///< Features origin frame.
ros
::
Time
features_stamp_
;
///< Time stamp from the features.
std
::
vector
<
FeatureInfo
>
features_
;
///< Feature data vector.
ros
::
Time
features_stamp_
;
///< Time stamp from the last features.
std
::
vector
<
FeatureInfo
>
features_
;
///< Both features data vector.
bool
new_front_features_data_
;
///< Bool to know that new front_features has been received.
std
::
string
front_features_frame_
;
///< front_features origin frame.
ros
::
Time
front_features_stamp_
;
///< Time stamp from the front_features.
std
::
vector
<
FeatureInfo
>
front_features_
;
///< Feature data vector.
bool
new_rear_features_data_
;
///< Bool to know that new rear_features has been received.
std
::
string
rear_features_frame_
;
///< rear_features origin frame.
ros
::
Time
rear_features_stamp_
;
///< Time stamp from the rear_features.
std
::
vector
<
FeatureInfo
>
rear_features_
;
///< Feature data vector.
std
::
list
<
LandmarkCandidate
>
landmarks_candidates_
;
///< Landmark candidates.
...
...
@@ -111,7 +121,7 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
Eigen
::
Vector3d
estimated_sigma_
;
///< Estimated main sigmas.
bool
reset_estimated_pose_
;
///< Boolena to set new_estimated_pose to false.
bool
last_frame_min_detections_
;
///< Flag to know that last frame was because a lot of features seen.
bool
last_frame_min_detections_
;
///< Flag to know that last frame was because a lot of
front_
features seen.
// [publisher attributes]
ros
::
Publisher
frame_data_publisher_
;
...
...
@@ -144,11 +154,17 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
tf2_ros
::
TransformListener
tf2_listener
;
ros
::
Subscriber
features_subscriber_
;
void
features_callback
(
const
iri_adc_msgs
::
feature_array
::
ConstPtr
&
msg
);
pthread_mutex_t
features_mutex_
;
void
features_mutex_enter
(
void
);
void
features_mutex_exit
(
void
);
ros
::
Subscriber
front_features_subscriber_
;
void
front_features_callback
(
const
iri_adc_msgs
::
feature_array
::
ConstPtr
&
msg
);
pthread_mutex_t
front_features_mutex_
;
void
front_features_mutex_enter
(
void
);
void
front_features_mutex_exit
(
void
);
ros
::
Subscriber
rear_features_subscriber_
;
void
rear_features_callback
(
const
iri_adc_msgs
::
feature_array
::
ConstPtr
&
msg
);
pthread_mutex_t
rear_features_mutex_
;
void
rear_features_mutex_enter
(
void
);
void
rear_features_mutex_exit
(
void
);
// [service attributes]
...
...
@@ -168,8 +184,10 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
/**
* \brief Function to remove landmark candidates not detected and reinit its variables.
*
* \param _front Flag to select front or rear features.
*/
void
prepare_landmark_candidates
(
void
);
void
prepare_landmark_candidates
(
bool
_front
);
/**
* \brief Function to analyze the feature information.
...
...
@@ -178,10 +196,12 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
* If enabled, it searchs for new landmarks.
*
* \param _update_landmarks Flag to know if it must search for new landmarks.
*
* \param _front Flag to select front or rear features.
*
* \return True on success.
*/
bool
check_landmarks
(
bool
_update_landmarks
);
bool
check_landmarks
(
bool
_update_landmarks
,
bool
_front
);
/**
* \brief Function to check if the feature is an existing landmark candidate and to update landmark candidates info.
...
...
@@ -191,10 +211,12 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
* If the feature isn't an existing landmark candidate, a new landmark candidate is added with its information.
*
* \param _feature The feature to check.
*
* \param _front Flag to select front or rear features.
*
* \return If is a landmark candidate have been seen enough times (is a landmark).
*/
bool
is_a_new_landmark
(
FeatureInfo
&
_feature
);
bool
is_a_new_landmark
(
FeatureInfo
&
_feature
,
bool
_front
);
/**
* \brief Function to update the frame data with the detected landmarks' information.
...
...
@@ -202,10 +224,12 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
* It transform The sensor information to base_link frame.
*
* \param _frame_data The frame data.
*
* \param _front Flag to select front or rear features.
*
* \return If succeded.
*/
bool
update_landmarks_frame_data
(
FrameData
&
_frame_data
);
bool
update_landmarks_frame_data
(
FrameData
&
_frame_data
,
bool
_front
);
/**
* \brief Function to update tf from map to odom at a requested time stamp.
...
...
@@ -238,6 +262,13 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
*/
bool
get_tf_map_odom
(
ros
::
Time
_t
,
Eigen
::
Vector3d
&
_tf_map_odom
);
/**
* \brief Function to merge features received.
*
* It only add features when they are not too old.
*/
void
merge_features
();
/**
* \brief Function to publish landmarks markers.
*/
...
...
launch/node.launch
View file @
f5f3d667
...
...
@@ -14,7 +14,8 @@
<!-- <arg name="topic_name" default="new_topic_name"/> -->
<arg
name=
"estimated_pose_topic_name"
default=
"~/estimated_pose"
/>
<arg
name=
"initialpose_topic_name"
default=
"~/initialpose"
/>
<arg
name=
"features_topic_name"
default=
"~/features"
/>
<arg
name=
"front_features_topic_name"
default=
"~/front_features"
/>
<arg
name=
"rear_features_topic_name"
default=
"~/rear_features"
/>
<node
name=
"$(arg node_name)"
...
...
@@ -30,7 +31,8 @@
<!--<remap from="~/topic" to="$(arg topic_name)"/>-->
<remap
from=
"~/estimated_pose"
to=
"$(arg estimated_pose_topic_name)"
/>
<remap
from=
"~/initialpose"
to=
"$(arg initialpose_topic_name)"
/>
<remap
from=
"~/features"
to=
"$(arg features_topic_name)"
/>
<remap
from=
"~/front_features"
to=
"$(arg front_features_topic_name)"
/>
<remap
from=
"~/rear_features"
to=
"$(arg rear_features_topic_name)"
/>
</node>
</launch>
src/adc_landmarks_slam_solver_alg_node.cpp
View file @
f5f3d667
This diff is collapsed.
Click to expand it.
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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