#include <EstimationNode.h>
Public Member Functions | |
void | comCb (const std_msgs::StringConstPtr str) |
void | dynConfCb (tum_ardrone::StateestimationParamsConfig &config, uint32_t level) |
EstimationNode () | |
void | Loop () |
void | navdataCb (const ardrone_autonomy::NavdataConstPtr navdataPtr) |
void | publishCommand (std::string c) |
void | publishTf (TooN::SE3<>, ros::Time stamp, int seq, std::string system) |
void | reSendInfo () |
void | toogleLogging () |
void | velCb (const geometry_msgs::TwistConstPtr velPtr) |
void | vidCb (const sensor_msgs::ImageConstPtr img) |
~EstimationNode () | |
Public Attributes | |
int | arDroneVersion |
std::string | calibFile |
long | currentLogID |
DroneKalmanFilter * | filter |
std::ofstream * | logfileFilter |
std::ofstream * | logfileIMU |
std::ofstream * | logfilePTAM |
std::ofstream * | logfilePTAMRaw |
MapView * | mapView |
std::string | packagePath |
PTAMWrapper * | ptamWrapper |
long | startedLogClock |
Static Public Attributes | |
static pthread_mutex_t | logFilter_CS = PTHREAD_MUTEX_INITIALIZER |
static pthread_mutex_t | logIMU_CS = PTHREAD_MUTEX_INITIALIZER |
static pthread_mutex_t | logPTAM_CS = PTHREAD_MUTEX_INITIALIZER |
static pthread_mutex_t | logPTAMRaw_CS = PTHREAD_MUTEX_INITIALIZER |
Private Attributes | |
std::string | command_channel |
std::string | control_channel |
ros::Publisher | dronepose_pub |
long | droneRosTSOffset |
long | lastDroneTS |
ardrone_autonomy::Navdata | lastNavdataReceived |
ros::Time | lastNavStamp |
long | lastRosTS |
std::string | navdata_channel |
ros::Subscriber | navdata_sub |
ros::NodeHandle | nh_ |
std::string | output_channel |
ros::Duration | predTime |
int | publishFreq |
tf::TransformBroadcaster | tf_broadcaster |
ros::Publisher | tum_ardrone_pub |
ros::Subscriber | tum_ardrone_sub |
ros::Subscriber | vel_sub |
ros::Subscriber | vid_sub |
std::string | video_channel |
Static Private Attributes | |
static pthread_mutex_t | tum_ardrone_CS = PTHREAD_MUTEX_INITIALIZER |
Definition at line 45 of file EstimationNode.h.
Definition at line 52 of file EstimationNode.cpp.
Definition at line 110 of file EstimationNode.cpp.
void EstimationNode::comCb | ( | const std_msgs::StringConstPtr | str | ) |
Definition at line 220 of file EstimationNode.cpp.
void EstimationNode::dynConfCb | ( | tum_ardrone::StateestimationParamsConfig & | config, |
uint32_t | level | ||
) |
Definition at line 303 of file EstimationNode.cpp.
void EstimationNode::Loop | ( | ) |
Definition at line 255 of file EstimationNode.cpp.
void EstimationNode::navdataCb | ( | const ardrone_autonomy::NavdataConstPtr | navdataPtr | ) |
Definition at line 120 of file EstimationNode.cpp.
void EstimationNode::publishCommand | ( | std::string | c | ) |
Definition at line 338 of file EstimationNode.cpp.
void EstimationNode::publishTf | ( | TooN::SE3<> | trans, |
ros::Time | stamp, | ||
int | seq, | ||
std::string | system | ||
) |
Definition at line 347 of file EstimationNode.cpp.
void EstimationNode::reSendInfo | ( | ) |
Definition at line 501 of file EstimationNode.cpp.
void EstimationNode::toogleLogging | ( | ) |
Definition at line 391 of file EstimationNode.cpp.
void EstimationNode::velCb | ( | const geometry_msgs::TwistConstPtr | velPtr | ) |
Definition at line 197 of file EstimationNode.cpp.
void EstimationNode::vidCb | ( | const sensor_msgs::ImageConstPtr | img | ) |
Definition at line 214 of file EstimationNode.cpp.
Definition at line 136 of file EstimationNode.h.
std::string EstimationNode::calibFile |
Definition at line 135 of file EstimationNode.h.
std::string EstimationNode::command_channel [private] |
Definition at line 80 of file EstimationNode.h.
std::string EstimationNode::control_channel [private] |
Definition at line 77 of file EstimationNode.h.
Definition at line 131 of file EstimationNode.h.
ros::Publisher EstimationNode::dronepose_pub [private] |
Definition at line 63 of file EstimationNode.h.
long EstimationNode::droneRosTSOffset [private] |
Definition at line 87 of file EstimationNode.h.
Definition at line 95 of file EstimationNode.h.
long EstimationNode::lastDroneTS [private] |
Definition at line 85 of file EstimationNode.h.
ardrone_autonomy::Navdata EstimationNode::lastNavdataReceived [private] |
Definition at line 91 of file EstimationNode.h.
ros::Time EstimationNode::lastNavStamp [private] |
Definition at line 52 of file EstimationNode.h.
long EstimationNode::lastRosTS [private] |
Definition at line 86 of file EstimationNode.h.
std::ofstream* EstimationNode::logfileFilter |
Definition at line 125 of file EstimationNode.h.
std::ofstream* EstimationNode::logfileIMU |
Definition at line 123 of file EstimationNode.h.
std::ofstream* EstimationNode::logfilePTAM |
Definition at line 124 of file EstimationNode.h.
std::ofstream* EstimationNode::logfilePTAMRaw |
Definition at line 126 of file EstimationNode.h.
pthread_mutex_t EstimationNode::logFilter_CS = PTHREAD_MUTEX_INITIALIZER [static] |
Definition at line 129 of file EstimationNode.h.
pthread_mutex_t EstimationNode::logIMU_CS = PTHREAD_MUTEX_INITIALIZER [static] |
Definition at line 127 of file EstimationNode.h.
pthread_mutex_t EstimationNode::logPTAM_CS = PTHREAD_MUTEX_INITIALIZER [static] |
Definition at line 128 of file EstimationNode.h.
pthread_mutex_t EstimationNode::logPTAMRaw_CS = PTHREAD_MUTEX_INITIALIZER [static] |
Definition at line 130 of file EstimationNode.h.
Definition at line 97 of file EstimationNode.h.
std::string EstimationNode::navdata_channel [private] |
Definition at line 76 of file EstimationNode.h.
ros::Subscriber EstimationNode::navdata_sub [private] |
Definition at line 49 of file EstimationNode.h.
ros::NodeHandle EstimationNode::nh_ [private] |
Definition at line 65 of file EstimationNode.h.
std::string EstimationNode::output_channel [private] |
Definition at line 78 of file EstimationNode.h.
std::string EstimationNode::packagePath |
Definition at line 98 of file EstimationNode.h.
ros::Duration EstimationNode::predTime [private] |
Definition at line 73 of file EstimationNode.h.
Definition at line 96 of file EstimationNode.h.
int EstimationNode::publishFreq [private] |
Definition at line 74 of file EstimationNode.h.
Definition at line 132 of file EstimationNode.h.
Definition at line 67 of file EstimationNode.h.
pthread_mutex_t EstimationNode::tum_ardrone_CS = PTHREAD_MUTEX_INITIALIZER [static, private] |
Definition at line 60 of file EstimationNode.h.
Definition at line 59 of file EstimationNode.h.
Definition at line 58 of file EstimationNode.h.
ros::Subscriber EstimationNode::vel_sub [private] |
Definition at line 50 of file EstimationNode.h.
ros::Subscriber EstimationNode::vid_sub [private] |
Definition at line 51 of file EstimationNode.h.
std::string EstimationNode::video_channel [private] |
Definition at line 79 of file EstimationNode.h.