1 #ifndef TUW_MARKER_SLAM_NODE_H 2 #define TUW_MARKER_SLAM_NODE_H 7 #include <geometry_msgs/PoseWithCovarianceStamped.h> 8 #include <geometry_msgs/Twist.h> 9 #include <marker_msgs/MarkerDetection.h> 10 #include <marker_msgs/MarkerWithCovarianceArray.h> 11 #include <dynamic_reconfigure/server.h> 14 #include "tuw_marker_slam/SLAMConfig.h" 15 #include "tuw_marker_slam/EKFSLAMConfig.h" 34 geometry_msgs::PoseWithCovarianceStamped
xt_;
35 marker_msgs::MarkerWithCovarianceArray
mt_;
56 #endif // TUW_MARKER_SLAM_NODE_H
std::string frame_id_map_
measurements are in x-z-plane (gazebo) instead of x-y-plane (stage)
ros::Subscriber sub_marker_
subscriber to the command
std::shared_ptr< tf::TransformListener > tf_listener_
broadcasts transformation messages
void callbackConfigSLAM(tuw_marker_slam::SLAMConfig &config, uint32_t level)
parameter server stuff general use
ros::Subscriber sub_cmd_
node handler to the current node
void callbackConfigEKFSLAM(tuw_marker_slam::EKFSLAMConfig &config, uint32_t level)
parameter server stuff for the EKF SLAM
ros::Publisher pub_xt_
subscriber to the marker detector
bool xzplane_
listener to receive transformation messages
ros::Publisher pub_mt_
publisher for the estimated vehicle pose and its covariance
std::string frame_id_base_
frame id of odom (for transformations)
void publish()
triggers the SLAM cycle
marker_msgs::MarkerWithCovarianceArray mt_
estimated vehicle pose and its covariance to publish
SLAMNode(ros::NodeHandle &n)
void callbackCmd(const geometry_msgs::Twist &)
frame id of base (for transformations)
geometry_msgs::PoseWithCovarianceStamped xt_
publisher for the estimated landmark poses and their covariances
std::string frame_id_odom_
frame id of map (for transformations)
dynamic_reconfigure::Server< tuw_marker_slam::SLAMConfig >::CallbackType reconfigureFncSLAM_
parameter server stuff general use
ros::NodeHandle n_param_
node handler to the root node
ros::NodeHandle n_
publishes the estimated landmark map and the estimated vehicle pose in it
dynamic_reconfigure::Server< tuw_marker_slam::SLAMConfig > reconfigureServerSLAM_
callback function to catch incoming marker data
dynamic_reconfigure::Server< tuw_marker_slam::EKFSLAMConfig >::CallbackType reconfigureFncEKFSLAM_
parameter server stuff for the EKF SLAM
void callbackMarker(const marker_msgs::MarkerDetection &)
callback function to catch motion commands
tf::TransformBroadcaster tf_broadcaster_
estimated landmark poses and their covariances to publish
std::shared_ptr< dynamic_reconfigure::Server< tuw_marker_slam::EKFSLAMConfig > > reconfigureServerEKFSLAM_
callback function on incoming parameter changes for general use