Go to the documentation of this file.00001 #include "covariance_markers_alg_node.h"
00002
00003 CovarianceMarkersAlgNode::CovarianceMarkersAlgNode(void)
00004 {
00005
00006 this->loop_rate_ = 2;
00007
00008
00009 this->markers_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("markers", 100);
00010
00011
00012 this->odom_subscriber_ = this->public_node_handle_.subscribe("odom", 100, &CovarianceMarkersAlgNode::odom_callback, this);
00013 new_marker_=false;
00014
00015
00016
00017
00018
00019
00020
00021
00022 }
00023
00024 CovarianceMarkersAlgNode::~CovarianceMarkersAlgNode(void)
00025 {
00026
00027 }
00028
00029 void CovarianceMarkersAlgNode::mainNodeThread(void)
00030 {
00031
00032
00033
00034
00035 MarkerArray_msg_ = alg_.marker_array();
00036
00037
00038
00039
00040 if(new_marker_)
00041 {
00042 this->markers_publisher_.publish(this->MarkerArray_msg_);
00043 new_marker_=false;
00044 }
00045 }
00046
00047
00048 void CovarianceMarkersAlgNode::odom_callback(const nav_msgs::Odometry & msg)
00049 {
00050
00051
00052 this->alg_.lock();
00053
00054
00055
00056 alg_.odom_ = msg;
00057 ROS_DEBUG("CovarianceMarkersAlgNode::odom_callback x: %f y: %f",
00058 alg_.odom_.pose.pose.position.x,
00059 alg_.odom_.pose.pose.position.y);
00060 if(!new_marker_)
00061 {
00062 alg_.drawCovariance();
00063 new_marker_=true;
00064 }
00065
00066
00067 this->alg_.unlock();
00068
00069 }
00070
00071
00072
00073
00074
00075
00076
00077 void CovarianceMarkersAlgNode::node_config_update(Config &config, uint32_t level)
00078 {
00079 }
00080
00081 void CovarianceMarkersAlgNode::addNodeDiagnostics(void)
00082 {
00083 }
00084
00085
00086 int main(int argc,char *argv[])
00087 {
00088 return algorithm_base::main<CovarianceMarkersAlgNode>(argc, argv, "covariance_markers_alg_node");
00089 }