Go to the documentation of this file.00001 #include "my_labeler_alg_node.h"
00002
00003 MyLabelerAlgNode::MyLabelerAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<MyLabelerAlgorithm>()
00005 {
00006
00007
00008
00009
00010
00011
00012 this->scan_subscriber_ = this->public_node_handle_.subscribe("scan", 100, &MyLabelerAlgNode::scan_callback, this);
00013 this->goal_subscriber_ = this->public_node_handle_.subscribe("goal", 100, &MyLabelerAlgNode::goal_callback, this);
00014
00015
00016
00017
00018 std::string homepath = getenv("HOME");
00019 string fileString = homepath + "/Desktop/social_forces_data/traj.txt";
00020 file_out_ = fopen ( fileString.c_str() , "w");
00021
00022
00023
00024
00025
00026 }
00027
00028 MyLabelerAlgNode::~MyLabelerAlgNode(void)
00029 {
00030
00031
00032 fclose(file_out_);
00033 }
00034
00035 void MyLabelerAlgNode::mainNodeThread(void)
00036 {
00037
00038
00039
00040
00041
00042
00043
00044 }
00045
00046
00047 void MyLabelerAlgNode::scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
00048 {
00049
00050
00051
00052
00053 this->alg_.lock();
00054 this->scan_mutex_.enter();
00055
00056
00057 bag_time_ = msg->header.stamp.toSec();
00058
00059 this->alg_.unlock();
00060 this->scan_mutex_.exit();
00061 }
00062 void MyLabelerAlgNode::goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
00063 {
00064
00065
00066
00067
00068
00069 ROS_INFO("x = %f , y = %f, t = %lf", msg->pose.position.x , msg->pose.position.y , bag_time_ );
00070
00071
00072 fprintf( file_out_ , " %f %f %lf\n", msg->pose.position.x , msg->pose.position.y , bag_time_ );
00073
00074
00075
00076
00077
00078 }
00079
00080
00081
00082
00083
00084
00085
00086 void MyLabelerAlgNode::node_config_update(Config &config, uint32_t level)
00087 {
00088 this->alg_.lock();
00089
00090 this->alg_.unlock();
00091 }
00092
00093 void MyLabelerAlgNode::addNodeDiagnostics(void)
00094 {
00095 }
00096
00097
00098 int main(int argc,char *argv[])
00099 {
00100 return algorithm_base::main<MyLabelerAlgNode>(argc, argv, "my_labeler_alg_node");
00101 }