jointspub_alg_node.cpp
Go to the documentation of this file.
00001 #include "jointspub_alg_node.h"
00002 #include "gazebo_msgs/SetModelState.h"
00003 #include "gazebo_msgs/GetModelState.h"
00004  
00005 JointspubAlgNode::JointspubAlgNode(void) :
00006   algorithm_base::IriBaseAlgorithm<JointspubAlgorithm>()
00007 {
00008   //init class attributes if necessary
00009   //this->loop_rate_ = 2;//in [Hz]
00010 
00011   // [init publishers]
00012   this->joints_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("arm_joints_states", 1);
00013 
00014   
00015   // [init subscribers]
00016   this->get_joints_subscriber_ = this->public_node_handle_.subscribe("get_link_states", 1, &JointspubAlgNode::get_joints_callback, this);
00017   
00018   // [init services]
00019  
00020   // [init clients]
00021   
00022   // [init action servers]
00023   
00024   // [init action clients]
00025 }
00026 
00027 JointspubAlgNode::~JointspubAlgNode(void)
00028 {
00029   // [free dynamic memory]
00030 }
00031 
00032 void JointspubAlgNode::mainNodeThread(void)
00033 {
00034   // [fill msg structures]
00035   
00036   // [fill srv structure and make request to the server]
00037     
00038   // [fill action structure and make request to the action server]
00039 
00040   // [publish messages]
00041 
00042 }
00043 
00044 /*  [subscriber callbacks] */
00045 void JointspubAlgNode::get_joints_callback(const gazebo_msgs::LinkStates::ConstPtr& msg) 
00046 { 
00047   
00048   //ROS_INFO("JointspubAlgNode::get_joints_callback: New Message Received");
00049   
00050   //use appropiate mutex to shared variables if necessary 
00051   //this->alg_.lock(); 
00052   this->get_joints_mutex_.enter(); 
00053   
00054   Joints_.header.stamp=ros::Time::now();
00055   Joints_.header.frame_id="";
00056   Joints_.name.resize(5);
00057   Joints_.position.resize(5);
00058   Joints_.velocity.resize(5);
00059   Joints_.effort.resize(5);
00060 
00061   for (int s=0; s<int(msg->name.size()); ++s){
00062 
00063     if (msg->name[s]=="quadrotor::arm_g1"){
00064       Joints_.position[0]=0.0;
00065       Joints_.velocity[0]=0.0;
00066       Joints_.effort[0]=100;
00067     }
00068     if (msg->name[s]=="quadrotor::arm_B1"){
00069       Joints_.name[1]=msg->name[s];
00070       Joints_.position[1]=0.0;
00071       Joints_.velocity[1]=0.0;
00072       Joints_.effort[1]=100;
00073     } 
00074     if (msg->name[s]=="quadrotor::arm_B2"){
00075       Joints_.name[2]=msg->name[s];
00076       Joints_.position[2]=1.571;
00077       Joints_.velocity[2]=0.0;
00078       Joints_.effort[2]=100;
00079     } 
00080     if (msg->name[s]=="quadrotor::arm_B3"){
00081       Joints_.name[3]=msg->name[s];
00082       Joints_.position[3]=1.571;
00083       Joints_.velocity[3]=0.0;
00084       Joints_.effort[3]=100;
00085     }
00086     if (msg->name[s]=="quadrotor::arm_B4"){
00087       Joints_.name[4]=msg->name[s];
00088       Joints_.position[4]=0.7071;
00089       Joints_.velocity[4]=0.0;
00090       Joints_.effort[4]=100;
00091     }
00092     this->joints_publisher_.publish(this->Joints_);
00093   }
00094   //unlock previously blocked shared variables 
00095   //this->alg_.unlock(); 
00096   this->get_joints_mutex_.exit(); 
00097 }
00098 
00099 /*  [service callbacks] */
00100 
00101 /*  [action callbacks] */
00102 
00103 /*  [action requests] */
00104 
00105 void JointspubAlgNode::node_config_update(Config &config, uint32_t level)
00106 {
00107   this->alg_.lock();
00108 
00109   this->alg_.unlock();
00110 }
00111 
00112 void JointspubAlgNode::addNodeDiagnostics(void)
00113 {
00114 }
00115 
00116 /* main function */
00117 int main(int argc,char *argv[])
00118 {
00119   return algorithm_base::main<JointspubAlgNode>(argc, argv, "jointspub_alg_node");
00120 }


iri_jointspub
Author(s): asantamaria
autogenerated on Fri Dec 6 2013 20:59:22