PositionSubscriber.cpp
Go to the documentation of this file.
00001 /*
00002  * PositionSubscriber.cpp
00003  *
00004  *  Created on: Dec 11, 2013
00005  *      Author: cwioro
00006  */
00007 
00008 #include "PositionSubscriber.h"
00009 #include <cmath>
00010 #include <string>
00011 
00012 PositionSubscriber::PositionSubscriber()
00013 {
00014     // TODO Auto-generated constructor stub
00015     initialized = false;
00016     x_pos_ = 0;
00017     y_pos_ = 0;
00018     callback_count = 0;
00019     callback_refresh = 4; // every 10 call of the subscribe f, the position will be updated.
00020 }
00021 
00022 PositionSubscriber::~PositionSubscriber()
00023 {
00024     // TODO Auto-generated destructor stub
00025 }
00026 
00027 void PositionSubscriber::Subscribe(const nav_msgs::Odometry::ConstPtr& position)
00028 {
00029     callback_count++;
00030 
00031     if (callback_count % callback_refresh == 0)
00032     {       
00033         initialized = true;
00034         this->x_pos_ = position->pose.pose.position.x;
00035         this->y_pos_ = position->pose.pose.position.y;
00036 
00037         //      x_pos_ = 7.3f;
00038     }
00039 }
00040 
00041 double PositionSubscriber::calcDistance(PositionSubscriber* other)
00042 { 
00043     //ROS_DEBUG("P1: %f / %f",this->x_pos_,this->y_pos_);
00044     //ROS_DEBUG("P2: %f / %f",other->x_pos_,other->y_pos_);
00045    // ROS_ERROR("me [%s] %u other [%s] %u", this->robot_name_.c_str(), initialized ,other->robot_name_.c_str() ,other->initialized);
00046     
00047     if (initialized && other->initialized)
00048         return sqrt(pow(x_pos_ - other->x_pos_, 2) + pow(y_pos_ - other->y_pos_, 2));
00049     else
00050         return -1;
00051 }
00052 
00053 


adhoc_communication
Author(s): Guenter Cwioro , Torsten Andre
autogenerated on Thu Jun 6 2019 20:59:43