00001 #include "tibi_dabo_laserscan_copy_alg_node.h" 00002 00003 TibiDaboLaserscanCopyAlgNode::TibiDaboLaserscanCopyAlgNode(void) : 00004 algorithm_base::IriBaseAlgorithm<TibiDaboLaserscanCopyAlgorithm>() 00005 { 00006 //init class attributes if necessary 00007 //this->loop_rate_ = 2;//in [Hz] 00008 00009 // [init publishers] 00010 this->scan_out_publisher_ = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scan_out", 100); 00011 00012 // [init subscribers] 00013 this->scan_subscriber_ = this->public_node_handle_.subscribe("scan", 100, &TibiDaboLaserscanCopyAlgNode::scan_callback, this); 00014 00015 // [init services] 00016 00017 // [init clients] 00018 00019 // [init action servers] 00020 00021 // [init action clients] 00022 } 00023 00024 TibiDaboLaserscanCopyAlgNode::~TibiDaboLaserscanCopyAlgNode(void) 00025 { 00026 // [free dynamic memory] 00027 } 00028 00029 void TibiDaboLaserscanCopyAlgNode::mainNodeThread(void) 00030 { 00031 // [fill msg structures] 00032 //this->LaserScan_msg_.data = my_var; 00033 00034 // [fill srv structure and make request to the server] 00035 00036 // [fill action structure and make request to the action server] 00037 00038 // [publish messages] 00039 //this->scan_out_publisher_.publish(this->LaserScan_msg_); 00040 } 00041 00042 /* [subscriber callbacks] */ 00043 void TibiDaboLaserscanCopyAlgNode::scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 00044 { 00045 ROS_INFO("TibiDaboLaserscanCopyAlgNode::scan_callback: New Message Received"); 00046 00047 //use appropiate mutex to shared variables if necessary 00048 //this->alg_.lock(); 00049 //this->scan_mutex_.enter(); 00050 this->LaserScan_msg_ = *msg; 00051 00052 this->scan_out_publisher_.publish(this->LaserScan_msg_); 00053 //std::cout << msg->data << std::endl; 00054 00055 //unlock previously blocked shared variables 00056 //this->alg_.unlock(); 00057 //this->scan_mutex_.exit(); 00058 } 00059 00060 /* [service callbacks] */ 00061 00062 /* [action callbacks] */ 00063 00064 /* [action requests] */ 00065 00066 void TibiDaboLaserscanCopyAlgNode::node_config_update(Config &config, uint32_t level) 00067 { 00068 this->alg_.lock(); 00069 00070 this->alg_.unlock(); 00071 } 00072 00073 void TibiDaboLaserscanCopyAlgNode::addNodeDiagnostics(void) 00074 { 00075 } 00076 00077 /* main function */ 00078 int main(int argc,char *argv[]) 00079 { 00080 return algorithm_base::main<TibiDaboLaserscanCopyAlgNode>(argc, argv, "tibi_dabo_laserscan_copy_alg_node"); 00081 }