dabo_arm_node_driver_node.cpp

Go to the documentation of this file.

#include "dabo_arm_node_driver_node.h"

DaboArmNodeDriverNode::DaboArmNodeDriverNode(ros::NodeHandle &nh) : 
  iri_base_driver::IriBaseNodeDriver<DaboArmNodeDriver>(nh),
  joint_motion_aserver_(public_node_handle_, "joint_motion"),
  motion_sequence_aserver_(public_node_handle_, "motion_sequence")
{
  XmlRpc::XmlRpcValue files_param_list;
  XmlRpc::XmlRpcValue param_list_item;
  unsigned int i=0;

  //init class attributes if necessary
  this->loop_rate_ = 100;//in [Hz]

  // [init publishers]
  this->joint_feedback_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("joint_feedback", 10);
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  joint_motion_aserver_.registerStartCallback(boost::bind(&DaboArmNodeDriverNode::joint_motionStartCallback, this, _1)); 
  joint_motion_aserver_.registerStopCallback(boost::bind(&DaboArmNodeDriverNode::joint_motionStopCallback, this)); 
  joint_motion_aserver_.registerIsFinishedCallback(boost::bind(&DaboArmNodeDriverNode::joint_motionIsFinishedCallback, this)); 
  joint_motion_aserver_.registerHasSucceedCallback(boost::bind(&DaboArmNodeDriverNode::joint_motionHasSucceedCallback, this)); 
  joint_motion_aserver_.registerGetResultCallback(boost::bind(&DaboArmNodeDriverNode::joint_motionGetResultCallback, this, _1)); 
  joint_motion_aserver_.registerGetFeedbackCallback(boost::bind(&DaboArmNodeDriverNode::joint_motionGetFeedbackCallback, this, _1)); 

  motion_sequence_aserver_.registerStartCallback(boost::bind(&DaboArmNodeDriverNode::motion_sequenceStartCallback, this, _1)); 
  motion_sequence_aserver_.registerStopCallback(boost::bind(&DaboArmNodeDriverNode::motion_sequenceStopCallback, this)); 
  motion_sequence_aserver_.registerIsFinishedCallback(boost::bind(&DaboArmNodeDriverNode::motion_sequenceIsFinishedCallback, this)); 
  motion_sequence_aserver_.registerHasSucceedCallback(boost::bind(&DaboArmNodeDriverNode::motion_sequenceHasSucceedCallback, this)); 
  motion_sequence_aserver_.registerGetResultCallback(boost::bind(&DaboArmNodeDriverNode::motion_sequenceGetResultCallback, this, _1)); 
  motion_sequence_aserver_.registerGetFeedbackCallback(boost::bind(&DaboArmNodeDriverNode::motion_sequenceGetFeedbackCallback, this, _1)); 
  
  // [init action clients]
  files_param_list.setSize(this->driver_.get_num_motion_seq_files());
  for(i=0;i<this->driver_.get_num_motion_seq_files();i++)
  {
    param_list_item=this->driver_.get_motion_seq_file(i);
    files_param_list[i]=param_list_item;
  }
  this->public_node_handle_.setParam("motion_files",files_param_list);
  files_param_list.clear();
  files_param_list.setSize(this->driver_.get_num_config_files());
  for(i=0;i<this->driver_.get_num_config_files();i++)
  {
    param_list_item=this->driver_.get_config_file(i);
    files_param_list[i]=param_list_item;
  }
  this->public_node_handle_.setParam("config_files",files_param_list);
  files_param_list.clear();

  this->public_node_handle_.getParam("side_id",this->side_id);
  this->JointState_msg_.name.clear();
  if(this->side_id=="left")
  {
    this->JointState_msg_.name.push_back("left_shoulder_yaw");
    this->JointState_msg_.name.push_back("left_shoulder_pitch");
    this->JointState_msg_.name.push_back("left_shoulder_roll");
    this->JointState_msg_.name.push_back("left_elbow");
  }
  else if(this->side_id=="right")
  {
    this->JointState_msg_.name.push_back("right_shoulder_yaw");
    this->JointState_msg_.name.push_back("right_shoulder_pitch");
    this->JointState_msg_.name.push_back("right_shoulder_roll");
    this->JointState_msg_.name.push_back("right_elbow");
  }
  else
  {
    this->JointState_msg_.name.resize(4);
  }
}

void DaboArmNodeDriverNode::mainNodeThread(void)
{
  std::vector<double> position(4);
  std::vector<double> velocity(4);
  unsigned int i=0;

  //lock access to driver if necessary
  this->driver_.lock();

  // update the loop rate
  this->loop_rate_ = this->driver_.get_feedback_rate();//in [Hz]

  // [fill msg Header if necessary]
  this->JointState_msg_.header.stamp = ros::Time::now();
  //<publisher_name>.header.frame_id = "<publisher_topic_name>";

  // [fill msg structures]
  this->driver_.get_position(position);
  this->driver_.get_velocity(velocity);
  this->JointState_msg_.position.clear();
  this->JointState_msg_.velocity.clear();
  for(i=0;i<position.size();i++)
  {
    // convert from degrees to radians
    this->JointState_msg_.position.push_back(position[i]*3.14159/180.0);
    this->JointState_msg_.velocity.push_back(velocity[i]*3.14159/180.0);
  }
  // [fill srv structure and make request to the server]

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

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

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

void DaboArmNodeDriverNode::check_motion_sequence_status(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
  CEventServer *event_server=CEventServer::instance();

  if(driver_.isRunning())
  {
    if(this->driver_.get_motion_sequence_complete_event_id()!="")
    {
      if(event_server->event_is_set(this->driver_.get_motion_sequence_complete_event_id()))
      {
        if(event_server->event_is_set(this->driver_.get_motion_sequence_error_event_id()))
        {
          stat.summary(1,"Sequence has finished with errors");
          stat.add("Reported error",this->driver_.get_motion_sequence_error_message());
        }
        else
        {
          stat.summary(0,"Sequence has finished ok");
        }
      }
      else
        stat.summary(0,"Sequence is in progress");
    }
    else
      stat.summary(0,"No sequence has been executed");
  }
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */
void DaboArmNodeDriverNode::joint_motionStartCallback(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal)
{
  control_msgs::FollowJointTrajectoryResult result;
  std::vector<double> current_position(4);
  std::vector<TDynamixelMotionStep> seq;
  unsigned int i=0,j=0;
  TDynamixelMotionStep step;

  //lock access to driver if necessary
  this->driver_.lock();

  if( driver_.isRunning() )
  {
    if(this->motion_sequence_aserver_.isActive())
      this->joint_motion_aserver_.setAborted(result,std::string("A motion sequence has been started"));
    else
    {
      try{
        this->driver_.get_position(current_position);
        // generate and execute the motion sequence
        for(i=0;i<goal->trajectory.points.size();i++)
        {
          step.position=current_position;
          step.velocity.resize(4,0.0);
          for(j=0;j<goal->trajectory.joint_names.size();j++)
          {
            if(this->side_id=="left")
            {
              if(goal->trajectory.joint_names[j]=="left_shoulder_yaw")
              {
                step.position[0]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
                step.velocity[0]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
              }
              else if(goal->trajectory.joint_names[j]=="left_shoulder_pitch")
              {
                step.position[1]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
                step.velocity[1]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
              }
              else if(goal->trajectory.joint_names[j]=="left_shoulder_roll")
              {
                step.position[2]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
                step.velocity[2]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
              }
              else if(goal->trajectory.joint_names[j]=="left_elbow")
              {
                step.position[3]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
                step.velocity[3]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
              }
            }
            else if(this->side_id=="right")
            {
              if(goal->trajectory.joint_names[j]=="right_shoulder_yaw")
              {
                step.position[0]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
                step.velocity[0]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
              }
              else if(goal->trajectory.joint_names[j]=="right_shoulder_pitch")
              {
                step.position[1]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
                step.velocity[1]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
              }
              else if(goal->trajectory.joint_names[j]=="Right_shoulder_roll")
              {
                step.position[2]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
                step.velocity[2]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
              }
              else if(goal->trajectory.joint_names[j]=="right_elbow")
              {
                step.position[3]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
                step.velocity[3]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
              }
            }
          }
          step.delay=goal->trajectory.points[i].time_from_start.toSec()*1000;
          seq.push_back(step);
          // set the next current position;
          current_position=step.position;
        }
        this->driver_.start_motion_sequence(seq);
      }catch(CException &e){
        this->joint_motion_aserver_.setAborted(result);
      }
    }
  }

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

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

  if( driver_.isRunning() )
  {
    try{
      this->driver_.stop();
    }catch(CException &e){
      this->driver_.unlock();
      throw;
    }
  }

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

bool DaboArmNodeDriverNode::joint_motionIsFinishedCallback(void) 
{ 
  CEventServer *event_server=CEventServer::instance();
  bool finished=false;

  //lock access to driver if necessary
  this->driver_.lock();

  if( driver_.isRunning() )
  {
    finished=event_server->event_is_set(this->driver_.get_motion_sequence_complete_event_id());
  }

  //unlock access to driver if necessary
  this->driver_.unlock();

  return finished;
} 

bool DaboArmNodeDriverNode::joint_motionHasSucceedCallback(void) 
{ 
  CEventServer *event_server=CEventServer::instance();

  //lock access to driver if necessary
  this->driver_.lock();
  bool success=false;

  if( driver_.isRunning() )
  {
    success=!event_server->event_is_set(this->driver_.get_motion_sequence_error_event_id());
  }

  //unlock access to driver if necessary
  this->driver_.unlock();

  return success;
} 

void DaboArmNodeDriverNode::joint_motionGetResultCallback(control_msgs::FollowJointTrajectoryResultPtr& result) 
{ 
  CEventServer *event_server=CEventServer::instance();

  //lock access to driver if necessary
  this->driver_.lock();

  if( driver_.isRunning() )
  {
    if(event_server->event_is_set(this->driver_.get_motion_sequence_error_event_id()))
      result->error_code=-1;
    else
      result->error_code=0;
  }
  
  //unlock access to driver if necessary
  this->driver_.unlock();
} 

void DaboArmNodeDriverNode::joint_motionGetFeedbackCallback(control_msgs::FollowJointTrajectoryFeedbackPtr& feedback) 
{ 
  std::vector<double> position,velocity;
  unsigned int i=0;

  //lock access to driver if necessary
  this->driver_.lock();

  if( driver_.isRunning() )
  {
    try{
      this->driver_.get_position(position);
      this->driver_.get_velocity(velocity);
      for(i=0;i<position.size();i++)
      {
        feedback->actual.positions.push_back(position[i]);
        feedback->actual.velocities.push_back(velocity[i]);
      }
    }catch(CException &e){
      this->driver_.unlock();
      throw;
    }
  }

  //unlock access to driver if necessary
  this->driver_.unlock();
}
void DaboArmNodeDriverNode::motion_sequenceStartCallback(const tibi_dabo_msgs::sequenceGoalConstPtr& goal)
{ 
  std::string file;
  tibi_dabo_msgs::sequenceResult result;

  //lock access to driver if necessary
  this->driver_.lock();

  if( driver_.isRunning() )
  {
    if(this->joint_motion_aserver_.isActive())
    {
      result.successful.push_back(false);
      result.observations.push_back(std::string("A joint motion is in progress"));
      this->motion_sequence_aserver_.setAborted(result);
    }
    else
    {
      try{
        // add the full path to the incomming filename
        ROS_INFO("Arm motion sequence file %s",goal->sequence_file[0].c_str());
        file=goal->sequence_file[0];
        this->driver_.start_motion_sequence(file);
      }catch(CException &e){
        result.successful.push_back(false);
        result.observations.push_back(e.what());
        this->motion_sequence_aserver_.setAborted(result);
      }
    }
  }

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

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

  if( driver_.isRunning() )
  {
    try{
      this->driver_.stop_motion_sequence();
    }catch(CException &e){
      this->driver_.unlock();
      throw;
    }
  }

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

bool DaboArmNodeDriverNode::motion_sequenceIsFinishedCallback(void) 
{ 
  CEventServer *event_server=CEventServer::instance();
  bool finished=false;

  //lock access to driver if necessary
  this->driver_.lock();

  if( driver_.isRunning() )
  {
    finished=event_server->event_is_set(this->driver_.get_motion_sequence_complete_event_id());
  }

  //unlock access to driver if necessary
  this->driver_.unlock();

  return finished;
} 

bool DaboArmNodeDriverNode::motion_sequenceHasSucceedCallback(void) 
{ 
  CEventServer *event_server=CEventServer::instance();
  bool success=false;

  //lock access to driver if necessary
  this->driver_.lock();

  if( driver_.isRunning() )
  {
    success=!event_server->event_is_set(this->driver_.get_motion_sequence_error_event_id());
  }

  //unlock access to driver if necessary
  this->driver_.unlock();

  return success;
} 

void DaboArmNodeDriverNode::motion_sequenceGetResultCallback(tibi_dabo_msgs::sequenceResultPtr& result) 
{ 
  CEventServer *event_server=CEventServer::instance();

  //lock access to driver if necessary
  this->driver_.lock();

  if( driver_.isRunning() )
  {
    result->successful.clear();
    result->observations.clear();
    result->successful.push_back(!event_server->event_is_set(this->driver_.get_motion_sequence_error_event_id()));
    result->observations.push_back(this->driver_.get_motion_sequence_error_message());
  }

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

void DaboArmNodeDriverNode::motion_sequenceGetFeedbackCallback(tibi_dabo_msgs::sequenceFeedbackPtr& feedback) 
{ 
  //lock access to driver if necessary
  this->driver_.lock();

  if( driver_.isRunning() )
  {
    try{
      feedback->percentage.clear();
      feedback->percentage.push_back(this->driver_.get_motion_sequence_completed_percentage());
    }catch(CException &e){
      this->driver_.unlock();
      throw;
    }
  }

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

/*  [action requests] */

void DaboArmNodeDriverNode::postNodeOpenHook(void)
{
  motion_sequence_aserver_.start();
  ROS_INFO("DaboArmDriverNode:: Motion sequence server started!");
  joint_motion_aserver_.start();
  ROS_INFO("DaboArmDriverNode:: Joint motion server started!");
}

void DaboArmNodeDriverNode::preNodeCloseHook(void)
{
  if(this->motion_sequence_aserver_.isStarted())
  {
    this->motion_sequence_aserver_.shutdown();
    ROS_INFO("DaboHeadDriverNode:: Motion sequence server stopped!");
  }
  if(this->joint_motion_aserver_.isStarted())
  {
    this->joint_motion_aserver_.shutdown();
    ROS_INFO("DaboHeadDriverNode:: Joint motion server stopped!");
  }
//  ros::shutdown();
}

void DaboArmNodeDriverNode::addNodeDiagnostics(void)
{
  diagnostic_.add("Motion sequence status", this, &DaboArmNodeDriverNode::check_motion_sequence_status);
}

void DaboArmNodeDriverNode::addNodeOpenedTests(void)
{
}

void DaboArmNodeDriverNode::addNodeStoppedTests(void)
{
}

void DaboArmNodeDriverNode::addNodeRunningTests(void)
{
}

void DaboArmNodeDriverNode::reconfigureNodeHook(int level)
{
}

DaboArmNodeDriverNode::~DaboArmNodeDriverNode(void)
{
  // [free dynamic memory]
  this->driver_.lock();
  // [free dynamic memory]
  if(this->motion_sequence_aserver_.isActive())
  {
    this->motion_sequence_aserver_.setAborted();
    this->driver_.stop_motion_sequence();
  }
  this->driver_.unlock();
}

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