fake_calibration_controller.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2013, Shadow Robot Company, All rights reserved.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 3.0 of the License, or (at your option) any later version.
 *
 * This library 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 for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library.
 */

#include "sr_ronex_controllers/fake_calibration_controller.hpp"
#include "pluginlib/class_list_macros.h"
#include <string>

PLUGINLIB_EXPORT_CLASS(ronex::FakeCalibrationController, controller_interface::ControllerBase)

namespace ronex
{
FakeCalibrationController::FakeCalibrationController()
  : robot_(NULL), last_publish_time_(0), state_(INITIALIZED),
    joint_(NULL)
{
}

bool FakeCalibrationController::init(ros_ethercat_model::RobotStateInterface* robot, ros::NodeHandle &n)
{
  robot_ = robot;
  node_ = n;
  // Joint

  std::string joint_name;
  if (!node_.getParam("joint", joint_name))
  {
    ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }

  std::string robot_state_name;
  node_.param<std::string>("robot_state_name", robot_state_name, "unique_robot_hw");

  try
  {
    joint_ = robot->getHandle(robot_state_name).getState()->getJointState(joint_name);
  }
  catch(const hardware_interface::HardwareInterfaceException& e)
  {
    ROS_ERROR_STREAM("Could not find robot state: " << robot_state_name << " Not loading the controller. " << e.what());
    return false;
  }

  if (joint_ == NULL)
  {
    ROS_ERROR("Could not find joint %s (namespace: %s)",
              joint_name.c_str(), node_.getNamespace().c_str());
    return false;
  }
  joint_name_ = joint_name;

  // "Calibrated" topic
  pub_calibrated_.reset(
    new realtime_tools::RealtimePublisher<std_msgs::Bool>(node_, "/calibrated", 1));

  return true;
}

void FakeCalibrationController::update(const ros::Time& time, const ros::Duration&)
{
  assert(joint_);

  switch (state_)
  {
  case INITIALIZED:
    state_ = BEGINNING;
    break;
  case BEGINNING:
    joint_->calibrated_ = true;
    calib_msg_.data = true;
    state_ = CALIBRATED;
    // We add the following line to delay for some time the first publish and allow the correct initialization of the
    // subscribers in calibrate.py
    last_publish_time_ = time;
    break;
  case CALIBRATED:
    if (pub_calibrated_)
    {
      if (last_publish_time_ + ros::Duration(0.5) < time)
      {
        assert(pub_calibrated_);
        if (pub_calibrated_->trylock())
        {
          last_publish_time_ = time;
          pub_calibrated_->msg_ = calib_msg_;
          pub_calibrated_->unlockAndPublish();
        }
      }
    }
    break;
  }
}
}  // namespace ronex

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/