kinton_arm_node.cpp

Go to the documentation of this file.

#include "kinton_arm_node.h"

using namespace std;

KintonArmDriverNode::KintonArmDriverNode(ros::NodeHandle &nh) : 
  iri_base_driver::IriBaseNodeDriver<KintonArmDriver>(nh)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  this->new_pos_.resize(5);
  this->new_pos_.reserve(5);
  this->new_pos_.clear();
  this->new_pos_.push_back(90);
  this->new_pos_.push_back(160);
  this->new_pos_.push_back(180);  
  this->new_pos_.push_back(20);
  this->new_pos_.push_back(90);

  this->old_pos_ = this->new_pos_;

  // [init publishers]
  this->arm_joint_feedback_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("arm_joint_feedback", 10);
  
  // [init subscribers]
  this->arm_joint_pos_subscriber_ = this->public_node_handle_.subscribe("arm_joint_pos", 10, &KintonArmDriverNode::arm_joint_pos_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

void KintonArmDriverNode::mainNodeThread(void)
{
  //lock access to driver if necessary
  this->driver_.lock();

  if (this->new_pos_ != this->old_pos_) 
  {
    this->driver_.set_new_positions(this->new_pos_);
  }

  this->old_pos_ = this->new_pos_;

  // [fill msg Header if necessary]
  this->JointState_msg_.header.stamp = ros::Time::now();
  // [fill msg structures]
  // Without feedback, the Joint state message is the same

  this->JointState_msg_.position.clear();
  this->JointState_msg_.velocity.clear();
  this->JointState_msg_.name.clear();

  this->JointState_msg_.name.push_back("joint1");
  this->JointState_msg_.name.push_back("joint2");
  this->JointState_msg_.name.push_back("joint3");
  this->JointState_msg_.name.push_back("joint4");
  this->JointState_msg_.name.push_back("joint5");

  for(size_t i=0;i<this->new_pos_.size();i++)
  {
    // convert from degrees to radians
    this->JointState_msg_.position.push_back(this->new_pos_[i]*3.14159/180.0);
    this->JointState_msg_.velocity.push_back(0.0);
  }  

  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
  this->arm_joint_feedback_publisher_.publish(this->JointState_msg_);

  //unlock access to driver if previously blocked
  this->driver_.unlock();
}

/*  [subscriber callbacks] */
void KintonArmDriverNode::arm_joint_pos_callback(const sensor_msgs::JointState::ConstPtr& msg) 
{ 
  //ROS_INFO("KintonArmDriverNode::arm_joint_pos_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  this->driver_.lock(); 
  //this->arm_joint_pos_mutex_.enter(); 

  //std::cout << msg->data << std::endl; 

  this->new_pos_.clear();
  this->new_pos_.push_back(msg->position[0]);
  this->new_pos_.push_back(msg->position[1]);
  this->new_pos_.push_back(msg->position[2]);  
  this->new_pos_.push_back(msg->position[3]);
  this->new_pos_.push_back(msg->position[4]);

   //unlock previously blocked shared variables 
  this->driver_.unlock(); 
  //this->arm_joint_pos_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void KintonArmDriverNode::postNodeOpenHook(void)
{
}

void KintonArmDriverNode::addNodeDiagnostics(void)
{
}

void KintonArmDriverNode::addNodeOpenedTests(void)
{
}

void KintonArmDriverNode::addNodeStoppedTests(void)
{
}

void KintonArmDriverNode::addNodeRunningTests(void)
{
}

void KintonArmDriverNode::reconfigureNodeHook(int level)
{
  // To move the servo positions manually using dynamic reconfigure
  
  this->driver_.lock(); 
  //this->arm_joint_pos_mutex_.enter(); 
  
  this->new_pos_.clear();
  this->new_pos_.push_back(this->driver_.config_.servo_1_pos);
  this->new_pos_.push_back(this->driver_.config_.servo_2_pos);
  this->new_pos_.push_back(this->driver_.config_.servo_3_pos);  
  this->new_pos_.push_back(this->driver_.config_.servo_4_pos);
  this->new_pos_.push_back(this->driver_.config_.servo_5_pos);
  
  this->driver_.unlock(); 
  //this->arm_joint_pos_mutex_.exit(); 
}

KintonArmDriverNode::~KintonArmDriverNode(void)
{
  // [free dynamic memory]
}

/* main function */
int main(int argc,char *argv[])
{
  return driver_base::main<KintonArmDriverNode>(argc, argv, "kinton_arm_driver_node");
}