#include <tuw_marker_slam_node.h>
Public Member Functions | |
void | cycle () |
constructor More... | |
void | publish () |
triggers the SLAM cycle More... | |
SLAMNode (ros::NodeHandle &n) | |
Public Member Functions inherited from tuw::SLAM | |
SLAM () | |
Private Member Functions | |
void | callbackCmd (const geometry_msgs::Twist &) |
frame id of base (for transformations) More... | |
void | callbackConfigEKFSLAM (tuw_marker_slam::EKFSLAMConfig &config, uint32_t level) |
parameter server stuff for the EKF SLAM More... | |
void | callbackConfigSLAM (tuw_marker_slam::SLAMConfig &config, uint32_t level) |
parameter server stuff general use More... | |
void | callbackMarker (const marker_msgs::MarkerDetection &) |
callback function to catch motion commands More... | |
Private Attributes | |
std::string | frame_id_base_ |
frame id of odom (for transformations) More... | |
std::string | frame_id_map_ |
measurements are in x-z-plane (gazebo) instead of x-y-plane (stage) More... | |
std::string | frame_id_odom_ |
frame id of map (for transformations) More... | |
marker_msgs::MarkerWithCovarianceArray | mt_ |
estimated vehicle pose and its covariance to publish More... | |
ros::NodeHandle | n_ |
publishes the estimated landmark map and the estimated vehicle pose in it More... | |
ros::NodeHandle | n_param_ |
node handler to the root node More... | |
ros::Publisher | pub_mt_ |
publisher for the estimated vehicle pose and its covariance More... | |
ros::Publisher | pub_xt_ |
subscriber to the marker detector More... | |
dynamic_reconfigure::Server< tuw_marker_slam::EKFSLAMConfig >::CallbackType | reconfigureFncEKFSLAM_ |
parameter server stuff for the EKF SLAM More... | |
dynamic_reconfigure::Server< tuw_marker_slam::SLAMConfig >::CallbackType | reconfigureFncSLAM_ |
parameter server stuff general use More... | |
std::shared_ptr< dynamic_reconfigure::Server< tuw_marker_slam::EKFSLAMConfig > > | reconfigureServerEKFSLAM_ |
callback function on incoming parameter changes for general use More... | |
dynamic_reconfigure::Server< tuw_marker_slam::SLAMConfig > | reconfigureServerSLAM_ |
callback function to catch incoming marker data More... | |
ros::Subscriber | sub_cmd_ |
node handler to the current node More... | |
ros::Subscriber | sub_marker_ |
subscriber to the command More... | |
tf::TransformBroadcaster | tf_broadcaster_ |
estimated landmark poses and their covariances to publish More... | |
std::shared_ptr< tf::TransformListener > | tf_listener_ |
broadcasts transformation messages More... | |
geometry_msgs::PoseWithCovarianceStamped | xt_ |
publisher for the estimated landmark poses and their covariances More... | |
bool | xzplane_ |
listener to receive transformation messages More... | |
Additional Inherited Members | |
Protected Member Functions inherited from tuw::SLAM | |
void | cycle () |
global parameters More... | |
Protected Attributes inherited from tuw::SLAM | |
cv::Mat_< double > | C_Yt_ |
combined state yt = (xt, mt_1, ..., mt_n) with xt = (x, y, alpha), mt_i = (x_i, y_i, alpha_i) More... | |
tuw_marker_slam::SLAMConfig | config_ |
technique used to estimate landmark map and the vehicles pose in it More... | |
unsigned long | loop_count_ |
measurements at time t More... | |
SLAMTechniquePtr | slam_technique_ |
counter for the triggered cycles More... | |
Command | ut_ |
combined covariance matrix of combined state More... | |
std::vector< Pose2D > | yt_ |
constructor More... | |
MeasurementPtr | zt_ |
motion commands v, w at time t More... | |
class to cover the ros communication for the slam
Definition at line 20 of file tuw_marker_slam_node.h.
SLAMNode::SLAMNode | ( | ros::NodeHandle & | n | ) |
Constructor
subscribes to command values
defines publishers for the resulting robot pose
defines publishers for the resulting landmark poses
subscribes to transforamtions
start parameter server
subscribes to marker detector
start parameter server
Definition at line 33 of file tuw_marker_slam_node.cpp.
|
private |
frame id of base (for transformations)
copies incoming robot command message
cmd |
Definition at line 210 of file tuw_marker_slam_node.cpp.
|
private |
parameter server stuff for the EKF SLAM
Definition at line 290 of file tuw_marker_slam_node.cpp.
|
private |
parameter server stuff general use
Definition at line 285 of file tuw_marker_slam_node.cpp.
|
private |
callback function to catch motion commands
copies incoming marker messages to the base class
marker |
Definition at line 219 of file tuw_marker_slam_node.cpp.
void SLAMNode::cycle | ( | ) |
constructor
Definition at line 118 of file tuw_marker_slam_node.cpp.
void SLAMNode::publish | ( | ) |
triggers the SLAM cycle
Publishes the estimated pose
Definition at line 129 of file tuw_marker_slam_node.cpp.
|
private |
frame id of odom (for transformations)
Definition at line 42 of file tuw_marker_slam_node.h.
|
private |
measurements are in x-z-plane (gazebo) instead of x-y-plane (stage)
Definition at line 40 of file tuw_marker_slam_node.h.
|
private |
frame id of map (for transformations)
Definition at line 41 of file tuw_marker_slam_node.h.
|
private |
estimated vehicle pose and its covariance to publish
Definition at line 35 of file tuw_marker_slam_node.h.
|
private |
publishes the estimated landmark map and the estimated vehicle pose in it
Definition at line 26 of file tuw_marker_slam_node.h.
|
private |
node handler to the root node
Definition at line 27 of file tuw_marker_slam_node.h.
|
private |
publisher for the estimated vehicle pose and its covariance
Definition at line 33 of file tuw_marker_slam_node.h.
|
private |
subscriber to the marker detector
Definition at line 32 of file tuw_marker_slam_node.h.
|
private |
parameter server stuff for the EKF SLAM
Definition at line 52 of file tuw_marker_slam_node.h.
|
private |
parameter server stuff general use
Definition at line 48 of file tuw_marker_slam_node.h.
|
private |
callback function on incoming parameter changes for general use
Definition at line 51 of file tuw_marker_slam_node.h.
|
private |
callback function to catch incoming marker data
Definition at line 47 of file tuw_marker_slam_node.h.
|
private |
node handler to the current node
Definition at line 29 of file tuw_marker_slam_node.h.
|
private |
subscriber to the command
Definition at line 30 of file tuw_marker_slam_node.h.
|
private |
estimated landmark poses and their covariances to publish
Definition at line 37 of file tuw_marker_slam_node.h.
|
private |
broadcasts transformation messages
Definition at line 38 of file tuw_marker_slam_node.h.
|
private |
publisher for the estimated landmark poses and their covariances
Definition at line 34 of file tuw_marker_slam_node.h.
|
private |
listener to receive transformation messages
Definition at line 39 of file tuw_marker_slam_node.h.