cob_head_axis.cpp

Go to the documentation of this file.

/****************************************************************
 *
 * Copyright (c) 2010
 *
 * Fraunhofer Institute for Manufacturing Engineering
 * and Automation (IPA)
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Project name: care-o-bot
 * ROS stack name: cob3_driver
 * ROS package name: cob_head_axis
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Author: Ulrich Reiser, email:ulrich.reiser@ipa.fhg.de
 *
 * Date of creation: Jan 2010
 * ToDo:
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of the Fraunhofer Institute for Manufacturing 
 *     Engineering and Automation (IPA) nor the names of its
 *     contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as 
 * published by the Free Software Foundation, either version 3 of the 
 * License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License LGPL for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public 
 * License LGPL along with this program. 
 * If not, see <http://www.gnu.org/licenses/>.
 *
 ****************************************************************/

//##################
//#### includes ####

// standard includes
//--

// ROS includes
#include <ros/ros.h>
#include <urdf/model.h>
#include <actionlib/server/simple_action_server.h>

// ROS message includes
#include <sensor_msgs/JointState.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <control_msgs/JointTrajectoryControllerState.h>
#include <diagnostic_msgs/DiagnosticArray.h>

// ROS service includes
#include <cob_srvs/Trigger.h>
#include <cob_srvs/SetOperationMode.h>
#include <cob_srvs/SetDefaultVel.h>

// external includes
#include <cob_head_axis/ElmoCtrl.h>

#include <unistd.h>

//####################
//#### node class ####
class NodeClass
{
  //
  public:
  // create a handle for this node, initialize node
  ros::NodeHandle n_;
    
  // declaration of topics to publish
  ros::Publisher topicPub_JointState_;
  ros::Publisher topicPub_ControllerState_;
  ros::Publisher topicPub_Diagnostic_;
  
  // declaration of topics to subscribe, callback is called for new messages arriving
  ros::Subscriber topicSub_JointCommand_;
  
  // declaration of service servers
  ros::ServiceServer srvServer_Init_;
  ros::ServiceServer srvServer_Stop_;
  ros::ServiceServer srvServer_Recover_;
  ros::ServiceServer srvServer_SetOperationMode_;
  ros::ServiceServer srvServer_SetDefaultVel_;
    
  // declaration of service clients
  //--

  // action lib server
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
  std::string action_name_;
  control_msgs::FollowJointTrajectoryFeedback feedback_;
  control_msgs::FollowJointTrajectoryResult result_;
  
  // global variables
  ElmoCtrl * CamAxis_;
  ElmoCtrlParams* CamAxisParams_;
  
  std::string CanDevice_;
  std::string CanIniFile_;
  int CanBaudrate_;
  int HomingDir_;
  int HomingDigIn_;
  double MaxVel_;
  int ModID_;
  std::string operationMode_;
  double LowerLimit_;
  double UpperLimit_; 
  double Offset_;
  int MotorDirection_;
  int EnoderIncrementsPerRevMot_;
  double GearRatio_;
  
  std::string JointName_;
  bool isInitialized_;
  bool isError_;
  bool finished_;
  double ActualPos_;
  double ActualVel_;
  double GoalPos_;
  trajectory_msgs::JointTrajectory traj_;
  trajectory_msgs::JointTrajectoryPoint traj_point_;
  unsigned int traj_point_nr_;

  // Constructor
  NodeClass(std::string name):
    as_(n_, name, boost::bind(&NodeClass::executeCB, this, _1), false),
    action_name_(name)
  {
    n_ = ros::NodeHandle("~");
    as_.start();

    isInitialized_ = false;
    isError_ = false;
    ActualPos_=0.0;
    ActualVel_=0.0;

    CamAxis_ = new ElmoCtrl();
    CamAxisParams_ = new ElmoCtrlParams();

    // implementation of topics to publish
    topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
    topicPub_ControllerState_ = n_.advertise<control_msgs::JointTrajectoryControllerState>("state", 1);
    topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);


    // implementation of topics to subscribe
    
    // implementation of service servers
    srvServer_Init_ = n_.advertiseService("init", &NodeClass::srvCallback_Init, this);
    srvServer_Stop_ = n_.advertiseService("stop", &NodeClass::srvCallback_Stop, this);
    srvServer_Recover_ = n_.advertiseService("recover", &NodeClass::srvCallback_Recover, this);
    srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &NodeClass::srvCallback_SetOperationMode, this);
    srvServer_SetDefaultVel_ = n_.advertiseService("set_default_vel", &NodeClass::srvCallback_SetDefaultVel, this);
    
    // implementation of service clients
    //--

    // read parameters from parameter server
    if(!n_.hasParam("EnoderIncrementsPerRevMot")) ROS_WARN("cob_head_axis: couldn't find parameter EnoderIncrementsPerRevMot, check if ALL parameters have been set correctly");

    n_.param<std::string>("CanDevice", CanDevice_, "PCAN");
    n_.param<int>("CanBaudrate", CanBaudrate_, 500);
    n_.param<int>("HomingDir", HomingDir_, 1);
    n_.param<int>("HomingDigIn", HomingDigIn_, 11);
    n_.param<int>("ModId",ModID_, 17);
    n_.param<std::string>("JointName",JointName_, "head_axis_joint");
    n_.param<std::string>("CanIniFile",CanIniFile_, "/");
    n_.param<std::string>("operation_mode",operationMode_, "position");
    n_.param<int>("MotorDirection",MotorDirection_, 1);
    n_.param<double>("GearRatio",GearRatio_, 62.5);
    n_.param<int>("EnoderIncrementsPerRevMot",EnoderIncrementsPerRevMot_, 4096);
    
    ROS_INFO("CanDevice=%s, CanBaudrate=%d, ModID=%d, HomingDigIn=%d",CanDevice_.c_str(),CanBaudrate_,ModID_,HomingDigIn_);
    
    
    // load parameter server string for robot/model
    std::string param_name = "/robot_description";
    std::string full_param_name;
    std::string xml_string;
    n_.searchParam(param_name,full_param_name);
    n_.getParam(full_param_name.c_str(),xml_string);
    ROS_INFO("full_param_name=%s",full_param_name.c_str());
    if (xml_string.size()==0)
    {
      ROS_ERROR("Unable to load robot model from param server robot_description\n");
      exit(2);
    }
    ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
    
    // extract limits and velocitys from urdf model
    urdf::Model model;
    if (!model.initString(xml_string))
    {
      ROS_ERROR("Failed to parse urdf file");
      exit(2);
    }
    ROS_INFO("Successfully parsed urdf file");

    // get LowerLimits out of urdf model
    LowerLimit_ = model.getJoint(JointName_.c_str())->limits->lower;
      //std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
    CamAxisParams_->SetLowerLimit(LowerLimit_);

    // get UpperLimits out of urdf model
    UpperLimit_ = model.getJoint(JointName_.c_str())->limits->upper;
      //std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
    CamAxisParams_->SetUpperLimit(UpperLimit_);

    // get Offset out of urdf model
    Offset_ = model.getJoint(JointName_.c_str())->calibration->rising.get()[0];
      //std::cout << "Offset[" << JointNames[i].c_str() << "] = " << Offsets[i] << std::endl;
    CamAxisParams_->SetAngleOffset(Offset_);
    
    // get velocitiy out of urdf model
    MaxVel_ = model.getJoint(JointName_.c_str())->limits->velocity;
    ROS_INFO("Successfully read limits from urdf");


    //initializing and homing of camera_axis
    CamAxisParams_->SetCanIniFile(CanIniFile_);
    CamAxisParams_->SetHomingDir(HomingDir_);
    CamAxisParams_->SetHomingDigIn(HomingDigIn_);
    CamAxisParams_->SetMaxVel(MaxVel_);
    CamAxisParams_->SetGearRatio(GearRatio_);
    CamAxisParams_->SetMotorDirection(MotorDirection_);
    CamAxisParams_->SetEncoderIncrements(EnoderIncrementsPerRevMot_);
    
    
    

    CamAxisParams_->Init(CanDevice_, CanBaudrate_, ModID_);
    

  }
  
  // Destructor
  ~NodeClass() 
  {
    delete CamAxis_;
  }

  void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
    if(isInitialized_) {
      ROS_INFO("Received new goal trajectory with %lu points",goal->trajectory.points.size());
      // saving goal into local variables
      traj_ = goal->trajectory;
      traj_point_nr_ = 0;
      traj_point_ = traj_.points[traj_point_nr_];
      GoalPos_ = traj_point_.positions[0];
      finished_ = false;
      
      // stoping axis to prepare for new trajectory
      CamAxis_->Stop();

      // check that preempt has not been requested by the client
      if (as_.isPreemptRequested())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        // set the action state to preempted
        as_.setPreempted();
      }
      
      usleep(2000000); // needed sleep until drive starts to change status from idle to moving
      
      while (not finished_)
      {
        if (as_.isNewGoalAvailable())
        {
          ROS_WARN("%s: Aborted", action_name_.c_str());
          as_.setAborted();
          return;
        }
          usleep(10000);
        //feedback_ = 
        //as_.send feedback_
      }

      // set the action state to succeed
      //result_.result.data = "executing trajectory";
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    
    } else {
      as_.setAborted();
      ROS_WARN("Camera_axis not initialized yet!");
    }
  }

  // service callback functions
  // function will be called when a service is querried
  bool srvCallback_Init(cob_srvs::Trigger::Request &req,
          cob_srvs::Trigger::Response &res )
  {
    if (isInitialized_ == false) {
      ROS_INFO("...initializing camera axis...");
      // init axis 
      if (CamAxis_->Init(CamAxisParams_))
      {
        CamAxis_->setGearPosVelRadS(0.0f, MaxVel_);
        ROS_INFO("Initializing of camera axis successfully");
        isInitialized_ = true;
        res.success.data = true;
        res.error_message.data = "initializing camera axis successfully";
      }
      else
      {
        ROS_ERROR("Initializing camera axis not successfully \n");
        res.success.data = false;
        res.error_message.data = "initializing camera axis not successfully";
      }
      }
      else
      {
        ROS_WARN("...camera axis already initialized...");
        res.success.data = true;
        res.error_message.data = "camera axis already initialized";
    }
    
    return true;
  }

  bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
          cob_srvs::Trigger::Response &res )
  {
    ROS_INFO("Stopping camera axis");
    if(isInitialized_)
    {
      // stopping all movements
      if (CamAxis_->Stop()) {
        ROS_INFO("Stopping camera axis successfully");
        res.success.data = true;
        res.error_message.data = "camera axis stopped successfully";
      }
      else {
        ROS_ERROR("Stopping camera axis not successfully. error");
        res.success.data = false;
        res.error_message.data = "stopping camera axis not successfully";
      }
    }
    return true;
  }
  
  bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
             cob_srvs::Trigger::Response &res )
  {
    if (isInitialized_) {
      ROS_INFO("Recovering camera axis");
      
      // stopping all arm movements
      if (CamAxis_->RecoverAfterEmergencyStop()) {
        ROS_INFO("Recovering camera axis successfully");
        res.success.data = true;
        res.error_message.data = "camera axis successfully recovered";
      } else {
        ROS_ERROR("Recovering camera axis not successfully. error");
        res.success.data = false;
        res.error_message.data = "recovering camera axis failed";
      }
    } else {
      ROS_WARN("...camera axis already recovered...");
      res.success.data = true;
      res.error_message.data = "camera axis already recovered";
    }

    return true;
  }

  bool srvCallback_SetOperationMode( cob_srvs::SetOperationMode::Request &req,
                    cob_srvs::SetOperationMode::Response &res )
  {
    ROS_INFO("Set operation mode to [%s]", req.operation_mode.data.c_str());
    n_.setParam("operation_mode", req.operation_mode.data.c_str());
    res.success.data = true; // 0 = true, else = false
    return true;
  }

  bool srvCallback_SetDefaultVel( cob_srvs::SetDefaultVel::Request &req,
                  cob_srvs::SetDefaultVel::Response &res )
  {
    ROS_INFO("Set default velocity to [%f]", req.default_vel);
    MaxVel_ = req.default_vel;
    CamAxisParams_->SetMaxVel(MaxVel_);
    CamAxis_->setMaxVelocity(MaxVel_);
    res.success.data = true; // 0 = true, else = false
    return true;
  }

  // other function declarations
  void updateCommands()
    {
      if (isInitialized_ == true)
      {
        if (operationMode_ == "position")
        {
          ROS_DEBUG("moving head_axis in position mode");

          if (fabs(ActualVel_) < 0.02)
          {
            //feedback_.isMoving = false;
        
            ROS_DEBUG("next point is %d from %lu",traj_point_nr_,traj_.points.size());
            
            if (traj_point_nr_ < traj_.points.size())
            {
              // if axis is not moving and not reached last point of trajectory, then send new target point
              ROS_INFO("...moving to trajectory point[%d], %f",traj_point_nr_,traj_.points[traj_point_nr_].positions[0]);
              traj_point_ = traj_.points[traj_point_nr_];
              CamAxis_->setGearPosVelRadS(traj_point_.positions[0], MaxVel_);
              usleep(900000);
              CamAxis_->m_Joint->requestPosVel();
              traj_point_nr_++;
              //feedback_.isMoving = true;
              //feedback_.pointNr = traj_point_nr;
              //as_.publishFeedback(feedback_);
            }
            else if ( fabs( fabs(ActualPos_) - fabs(GoalPos_) ) < 0.5*M_PI/180.0 && !finished_ )
            {
              ROS_DEBUG("...reached end of trajectory");
              finished_ = true;
            }
            else
            {
              //do nothing until GoalPos_ is reached
            }
          }
          else
          {
            ROS_DEBUG("...axis still moving to point[%d]",traj_point_nr_);
          }
        }
        else if (operationMode_ == "velocity")
        {
          ROS_WARN("Moving in velocity mode currently disabled");
        }
        else
        {
          ROS_ERROR("axis neither in position nor in velocity mode. OperationMode = [%s]", operationMode_.c_str());
        }
      }
      else
      {
        ROS_DEBUG("axis not initialized");
      }
    }
  
  void publishJointState()
  {

    if (isInitialized_ == true) {
      isError_ = CamAxis_->isError();

      // create message
      int DOF = 1;

      CamAxis_->evalCanBuffer();
      CamAxis_->getGearPosVelRadS(&ActualPos_,&ActualVel_);
      CamAxis_->m_Joint->requestPosVel();

      // really bad hack
      ActualPos_ = HomingDir_ * ActualPos_;
      ActualVel_ = HomingDir_ * ActualVel_;

      sensor_msgs::JointState msg;
      msg.header.stamp = ros::Time::now();
      msg.name.resize(DOF);
      msg.position.resize(DOF);
      msg.velocity.resize(DOF);
      msg.effort.resize(DOF);
      
      msg.name[0] = JointName_;
      msg.position[0] = ActualPos_;
      msg.velocity[0] = ActualVel_;
      msg.effort[0] = 0.0;


      //std::cout << "Joint " << msg.name[0] <<": pos="<<  msg.position[0] << " vel=" << msg.velocity[0] << std::endl;
        
      // publish message
      topicPub_JointState_.publish(msg);

      // publish controller state message
      control_msgs::JointTrajectoryControllerState controllermsg;
      controllermsg.header = msg.header;
      controllermsg.joint_names.resize(DOF);
      controllermsg.desired.positions.resize(DOF);
      controllermsg.desired.velocities.resize(DOF);
      controllermsg.actual.positions.resize(DOF);
      controllermsg.actual.velocities.resize(DOF);
      controllermsg.error.positions.resize(DOF);
      controllermsg.error.velocities.resize(DOF);
      
      controllermsg.joint_names = msg.name;
      controllermsg.desired.positions = msg.position;
      controllermsg.desired.velocities = msg.velocity;
      controllermsg.actual.positions = msg.position;
      controllermsg.actual.velocities = msg.velocity;
      // error, calculated out of desired and actual values
      for (int i = 0; i<DOF; i++ )
      {
        controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
        controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
      }
      topicPub_ControllerState_.publish(controllermsg);
    }
    // publishing diagnotic messages
      diagnostic_msgs::DiagnosticArray diagnostics;
      diagnostics.status.resize(1);
      // set data to diagnostics
      if(isError_)
      {
        diagnostics.status[0].level = 2;
        diagnostics.status[0].name = n_.getNamespace();
        diagnostics.status[0].message = "drive is in error mode";
      }
      else
      {
        if (isInitialized_)
        {
          diagnostics.status[0].level = 0;
          diagnostics.status[0].name = n_.getNamespace();
          diagnostics.status[0].message = "head axis initialized and running";
        }
        else
        {
          diagnostics.status[0].level = 1;
          diagnostics.status[0].name = n_.getNamespace();
          diagnostics.status[0].message = "head axis not initialized";
        }
      }
      // publish diagnostic message
      topicPub_Diagnostic_.publish(diagnostics);
  }

}; //NodeClass


//#######################
//#### main programm ####
int main(int argc, char** argv)
{
  // initialize ROS, spezify name of node
  ros::init(argc, argv, "cob_camera_axis");
  
  // create nodeClass
  //NodeClass nodeClass(ros::this_node::getName() + "/joint_trajectory_action");
  NodeClass nodeClass(ros::this_node::getName() + "/follow_joint_trajectory");
 
  // main loop
  ros::Rate loop_rate(10); // Hz
  while(nodeClass.n_.ok()) {
    
    // publish JointState
    nodeClass.publishJointState();

    // update commands
    nodeClass.updateCommands();

    // sleep and waiting for messages, callbacks 
    ros::spinOnce();
    loop_rate.sleep();
  }
  
  return 0;
}