00001 #include "trajectory_to_markers_alg_node.h" 00002 00003 TrajectoryToMarkersAlgNode::TrajectoryToMarkersAlgNode(void) : 00004 algorithm_base::IriBaseAlgorithm<TrajectoryToMarkersAlgorithm>() 00005 { 00006 initialized = false; 00007 //init class attributes if necessary 00008 //this->loop_rate_ = 2;//in [Hz] 00009 00010 // [init publishers] 00011 this->CovarianceMarkers_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("CovarianceMarkers", 1); 00012 this->ActualPoseMarker_publisher_ = this->public_node_handle_.advertise<visualization_msgs::Marker>("ActualPoseMarker", 1); 00013 00014 // [init subscribers] 00015 this->trajectory_subscriber_ = this->public_node_handle_.subscribe("trajectory", 100, &TrajectoryToMarkersAlgNode::trajectory_callback, this); 00016 00017 // [init services] 00018 00019 // [init clients] 00020 00021 // [init action servers] 00022 00023 // [init action clients] 00024 } 00025 00026 TrajectoryToMarkersAlgNode::~TrajectoryToMarkersAlgNode(void) 00027 { 00028 // [free dynamic memory] 00029 } 00030 00031 void TrajectoryToMarkersAlgNode::mainNodeThread(void) 00032 { 00033 00034 if (!initialized) 00035 { 00036 this->alg_.lock(); 00037 alg_.initialize_markers(); 00038 this->alg_.unlock(); 00039 initialized = true; 00040 } 00041 // [fill msg structures] 00042 // [fill srv structure and make request to the server] 00043 // [fill action structure and make request to the action server] 00044 // [publish messages] 00045 } 00046 00047 /* [subscriber callbacks] */ 00048 void TrajectoryToMarkersAlgNode::trajectory_callback(const iri_nav_msgs::Trajectory::ConstPtr& msg) 00049 { 00050 //ROS_INFO("TrajectoryToMarkersAlgNode::trajectory_callback: New Message Received"); 00051 //ROS_INFO_STREAM(*msg); 00052 00053 //use appropiate mutex to shared variables if necessary 00054 this->alg_.lock(); 00055 // draw covariances and if new trajectory, publish it 00056 if (alg_.drawCovariances(*msg)) 00057 this->CovarianceMarkers_publisher_.publish(alg_.get_marker_array()); 00058 00059 // Publishing actual marker (changed for sure) 00060 this->ActualPoseMarker_publisher_.publish(alg_.get_actual_marker()); 00061 //unlock previously blocked shared variables 00062 this->alg_.unlock(); 00063 00064 //this->trajectory_mutex_.exit(); 00065 } 00066 00067 /* [service callbacks] */ 00068 00069 /* [action callbacks] */ 00070 00071 /* [action requests] */ 00072 00073 void TrajectoryToMarkersAlgNode::node_config_update(Config &config, uint32_t level) 00074 { 00075 this->alg_.lock(); 00076 00077 this->alg_.unlock(); 00078 } 00079 00080 void TrajectoryToMarkersAlgNode::addNodeDiagnostics(void) 00081 { 00082 } 00083 00084 /* main function */ 00085 int main(int argc,char *argv[]) 00086 { 00087 return algorithm_base::main<TrajectoryToMarkersAlgNode>(argc, argv, "trajectory_to_markers_alg_node"); 00088 }