posewithcovariancestamped_odom_alg_node.cpp
Go to the documentation of this file.
00001 #include "posewithcovariancestamped_odom_alg_node.h"
00002 
00003 PosewithcovariancestampedOdomAlgNode::PosewithcovariancestampedOdomAlgNode(void) :
00004   algorithm_base::IriBaseAlgorithm<PosewithcovariancestampedOdomAlgorithm>()
00005 {
00006   //init class attributes if necessary
00007   //this->loop_rate_ = 2;//in [Hz]
00008 
00009   // [init publishers]
00010   this->odom_publisher_ = this->public_node_handle_.advertise<nav_msgs::Odometry>("odom", 100);
00011 
00012   // [init subscribers]
00013   this->pose_subscriber_ = this->public_node_handle_.subscribe("pose", 100, &PosewithcovariancestampedOdomAlgNode::pose_callback, this);
00014 
00015   this->public_node_handle_.param<std::string>("child_id", child_id_, "base_footprint");
00016   this->public_node_handle_.param<std::string>("parent_id", parent_id_, "odom");
00017   this->public_node_handle_.param<bool>("publish_tf", publish_tf_, true);
00018 
00019   // [init services]
00020 
00021   // [init clients]
00022 
00023   // [init action servers]
00024 
00025   // [init action clients]
00026 }
00027 
00028 PosewithcovariancestampedOdomAlgNode::~PosewithcovariancestampedOdomAlgNode(void)
00029 {
00030   // [free dynamic memory]
00031 }
00032 
00033 void PosewithcovariancestampedOdomAlgNode::mainNodeThread(void)
00034 {
00035   // [fill msg structures]
00036   //this->Odometry_msg.data = my_var;
00037 
00038   // [fill srv structure and make request to the server]
00039 
00040   // [fill action structure and make request to the action server]
00041 
00042   // [publish messages]
00043 
00044 }
00045 
00046 /*  [subscriber callbacks] */
00047 void PosewithcovariancestampedOdomAlgNode::pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
00048 {
00049   //ROS_DEBUG("PosewithcovariancestampedOdomAlgNode::pose_callback: New Message Received");
00050 
00051   //use appropiate mutex to shared variables if necessary
00052   //this->alg_.lock();
00053   //this->pose_mutex_.enter();
00054 
00055   //std::cout << msg->data << std::endl;
00056   this->Odometry_msg_.header = msg->header;
00057   this->Odometry_msg_.header.frame_id = parent_id_;
00058   this->Odometry_msg_.child_frame_id = child_id_;
00059   this->Odometry_msg_.pose   = msg->pose;
00060 
00061   this->odom_publisher_.publish(this->Odometry_msg_);
00062 
00063   //unlock previously blocked shared variables
00064   //this->alg_.unlock();
00065   //this->pose_mutex_.exit();
00066 }
00067 
00068 /*  [service callbacks] */
00069 
00070 /*  [action callbacks] */
00071 
00072 /*  [action requests] */
00073 
00074 void PosewithcovariancestampedOdomAlgNode::node_config_update(Config &config, uint32_t level)
00075 {
00076   this->alg_.lock();
00077 
00078   this->alg_.unlock();
00079 }
00080 
00081 void PosewithcovariancestampedOdomAlgNode::addNodeDiagnostics(void)
00082 {
00083 }
00084 
00085 /* main function */
00086 int main(int argc,char *argv[])
00087 {
00088   return algorithm_base::main<PosewithcovariancestampedOdomAlgNode>(argc, argv, "posewithcovariancestamped_odom_alg_node");
00089 }


iri_msg_to_odom
Author(s): Marti Morta, mmorta at iri.upc.edu
autogenerated on Fri Dec 6 2013 22:08:38