Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00025 #include "sr_ronex_controllers/fake_calibration_controller.hpp"
00026 #include "pluginlib/class_list_macros.h"
00027 #include <string>
00028
00029 PLUGINLIB_EXPORT_CLASS(ronex::FakeCalibrationController, controller_interface::ControllerBase)
00030
00031 namespace ronex
00032 {
00033 FakeCalibrationController::FakeCalibrationController()
00034 : robot_(NULL), last_publish_time_(0), state_(INITIALIZED),
00035 joint_(NULL)
00036 {
00037 }
00038
00039 bool FakeCalibrationController::init(ros_ethercat_model::RobotStateInterface* robot, ros::NodeHandle &n)
00040 {
00041 robot_ = robot;
00042 node_ = n;
00043
00044
00045 std::string joint_name;
00046 if (!node_.getParam("joint", joint_name))
00047 {
00048 ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00049 return false;
00050 }
00051
00052 std::string robot_state_name;
00053 node_.param<std::string>("robot_state_name", robot_state_name, "unique_robot_hw");
00054
00055 try
00056 {
00057 joint_ = robot->getHandle(robot_state_name).getState()->getJointState(joint_name);
00058 }
00059 catch(const hardware_interface::HardwareInterfaceException& e)
00060 {
00061 ROS_ERROR_STREAM("Could not find robot state: " << robot_state_name << " Not loading the controller. " << e.what());
00062 return false;
00063 }
00064
00065 if (joint_ == NULL)
00066 {
00067 ROS_ERROR("Could not find joint %s (namespace: %s)",
00068 joint_name.c_str(), node_.getNamespace().c_str());
00069 return false;
00070 }
00071 joint_name_ = joint_name;
00072
00073
00074 pub_calibrated_.reset(
00075 new realtime_tools::RealtimePublisher<std_msgs::Bool>(node_, "/calibrated", 1));
00076
00077 return true;
00078 }
00079
00083 void FakeCalibrationController::update(const ros::Time& time, const ros::Duration&)
00084 {
00085 assert(joint_);
00086
00087 switch (state_)
00088 {
00089 case INITIALIZED:
00090 state_ = BEGINNING;
00091 break;
00092 case BEGINNING:
00093 joint_->calibrated_ = true;
00094 calib_msg_.data = true;
00095 state_ = CALIBRATED;
00096
00097
00098 last_publish_time_ = time;
00099 break;
00100 case CALIBRATED:
00101 if (pub_calibrated_)
00102 {
00103 if (last_publish_time_ + ros::Duration(0.5) < time)
00104 {
00105 assert(pub_calibrated_);
00106 if (pub_calibrated_->trylock())
00107 {
00108 last_publish_time_ = time;
00109 pub_calibrated_->msg_ = calib_msg_;
00110 pub_calibrated_->unlockAndPublish();
00111 }
00112 }
00113 }
00114 break;
00115 }
00116 }
00117 }
00118
00119
00120
00121
00122
00123