Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _trajectory_2_markers_alg_node_h_
00026 #define _trajectory_2_markers_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "trajectory_2_markers_alg.h"
00030
00031 #include <Eigen/Core>
00032 #include <Eigen/Eigenvalues>
00033
00034
00035 #include <iri_poseslam/Trajectory.h>
00036 #include <visualization_msgs/MarkerArray.h>
00037 #include <visualization_msgs/Marker.h>
00038 #include <tf/transform_broadcaster.h>
00039 #include <tf/transform_datatypes.h>
00040
00041
00042
00043
00044
00049 class Trajectory2MarkersAlgNode : public algorithm_base::IriBaseAlgorithm<Trajectory2MarkersAlgorithm>
00050 {
00051 private:
00052
00053 ros::Publisher CovarianceMarkers_publisher_;
00054 ros::Publisher TrajectoryMarkers_publisher_;
00055 ros::Publisher ActualPoseMarker_publisher_;
00056
00057 visualization_msgs::MarkerArray covariance_markers_;
00058 visualization_msgs::Marker loops_marker_;
00059 visualization_msgs::Marker trajectory_marker_;
00060 visualization_msgs::Marker actual_marker_;
00061
00062
00063 ros::Subscriber trajectory_subscriber_;
00064 void trajectory_callback(const iri_poseslam::Trajectory::ConstPtr& msg);
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074 std::vector<bool> loop_step_;
00075 uint nLoops_;
00076
00077 Config config_;
00078
00079 public:
00086 Trajectory2MarkersAlgNode(void);
00087
00094 ~Trajectory2MarkersAlgNode(void);
00095
00096 protected:
00109 void mainNodeThread(void);
00110
00123 void node_config_update(Config &config, uint32_t level);
00124
00131 void addNodeDiagnostics(void);
00132
00133
00134
00137 void initialize_markers();
00138
00141 void drawTrajectory(const iri_poseslam::Trajectory& msg);
00142
00145 visualization_msgs::MarkerArray get_trajectory_marker() const;
00146
00149 visualization_msgs::Marker get_actual_marker() const;
00150
00153 visualization_msgs::MarkerArray get_covariance_markers() const;
00154
00157 visualization_msgs::Marker create_marker(const uint& id, const std_msgs::Header& header, const Eigen::Matrix2d& covs, const double& theta_cov, const geometry_msgs::Point& position, const bool& loopClosure) const;
00158
00161 void change_actual_marker(const Eigen::Matrix2d& covs, const double& theta_cov, const geometry_msgs::Point& position, const bool& loopClosure);
00162
00165 Eigen::Matrix2d get_ith_cov(const iri_poseslam::Trajectory& msg, const uint i) const;
00166
00169 double get_ith_theta_cov(const iri_poseslam::Trajectory& msg, const uint i) const;
00170 };
00171
00172 #endif