joint_movement_action_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2010  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * joint_movement_action_controller.cpp
00020  *
00021  *  Created on: 07.04.2011
00022  *      Author: Henning Deeken <hdeeken@uos.de>
00023  *
00024  */
00025 
00026 #include "katana/joint_movement_action_controller.h"
00027 #include <fstream>
00028 #include <iostream>
00029 #include <cstdio>
00030 
00031 namespace katana
00032 {
00033 
00034 JointMovementActionController::JointMovementActionController(boost::shared_ptr<AbstractKatana> katana) :
00035   katana_(katana), action_server_(ros::NodeHandle(), "katana_arm_controller/joint_movement_action",
00036                                   boost::bind(&JointMovementActionController::executeCB, this, _1), false)
00037 {
00038   joints_ = katana_->getJointNames();
00039   gripper_joints_ = katana_->getGripperJointNames();
00040   action_server_.start();
00041 }
00042 
00043 JointMovementActionController::~JointMovementActionController()
00044 {
00045 }
00046 
00050 bool JointMovementActionController::suitableJointGoal(const std::vector<std::string> &jointGoalNames)
00051 {
00052   for (size_t i = 0; i < jointGoalNames.size(); i++)
00053   {
00054     bool exists = false;
00055 
00056     for (size_t j = 0; j < joints_.size(); j++)
00057     {
00058       if (jointGoalNames[i] == joints_[j])
00059         exists = true;
00060     }
00061 
00062     for (size_t k = 0; k < gripper_joints_.size(); k++)
00063     {
00064       if (jointGoalNames[i] == gripper_joints_[k])
00065         exists = true;
00066     }
00067     if (!exists)
00068     {
00069       ROS_ERROR("joint name %s is not one of our controlled joints", jointGoalNames[i].c_str());
00070       return false;
00071     }
00072   }
00073 
00074   return true;
00075 }
00076 
00077 sensor_msgs::JointState JointMovementActionController::adjustJointGoalPositionsToMotorLimits(
00078                                                                                              const sensor_msgs::JointState &jointGoal)
00079 {
00080   sensor_msgs::JointState adjustedJointGoal;
00081 
00082   adjustedJointGoal.name = jointGoal.name;
00083   adjustedJointGoal.position = jointGoal.position;
00084 
00085   for (size_t i = 0; i < jointGoal.name.size(); i++)
00086   {
00087     ROS_DEBUG("%s - min: %f - max: %f - curr: % f - req: %f", jointGoal.name[i].c_str(), katana_->getMotorLimitMin(jointGoal.name[i]), katana_->getMotorLimitMax(jointGoal.name[i]), katana_->getMotorAngles()[katana_->getJointIndex(jointGoal.name[i])], jointGoal.position[i]);
00088   }
00089 
00090   for (size_t i = 0; i < jointGoal.name.size(); i++)
00091   {
00092 
00093     if (jointGoal.position[i] < katana_->getMotorLimitMin(jointGoal.name[i]))
00094     {
00095       adjustedJointGoal.position[i] = katana_->getMotorLimitMin(jointGoal.name[i]);
00096 
00097       ROS_INFO("%s - requested JointGoalPosition: %f exceeded MotorLimit: %f  - adjusted JointGoalPosition to MotorLimit", jointGoal.name[i].c_str(), jointGoal.position[i], katana_->getMotorLimitMin(jointGoal.name[i]));
00098     }
00099 
00100     if (jointGoal.position[i] > katana_->getMotorLimitMax(jointGoal.name[i]))
00101     {
00102       adjustedJointGoal.position[i] = katana_->getMotorLimitMax(jointGoal.name[i]);
00103       ROS_INFO("%s - requested JointGoalPosition: %f exceeded MotorLimit: %f  - adjusted JointGoalPosition to MotorLimit", jointGoal.name[i].c_str(), jointGoal.position[i], katana_->getMotorLimitMax(jointGoal.name[i]));
00104     }
00105   }
00106 
00107   return adjustedJointGoal;
00108 }
00109 
00110 void JointMovementActionController::executeCB(const JMAS::GoalConstPtr &goal)
00111 {
00112   // note: the SimpleActionServer guarantees that we enter this function only when
00113   // there is no other active goal. in other words, only one instance of executeCB()
00114   // is ever running at the same time.
00115 
00116   if (!suitableJointGoal(goal->jointGoal.name))
00117   {
00118     ROS_ERROR("Joints on incoming goal don't match our joints/gripper_joints");
00119 
00120     for (size_t i = 0; i < goal->jointGoal.name.size(); i++)
00121     {
00122       ROS_INFO("  incoming joint %d: %s", (int)i, goal->jointGoal.name[i].c_str());
00123     }
00124 
00125     for (size_t i = 0; i < joints_.size(); i++)
00126     {
00127       ROS_INFO("  our joint      %d: %s", (int)i, joints_[i].c_str());
00128     }
00129 
00130     for (size_t i = 0; i < gripper_joints_.size(); i++)
00131     {
00132       ROS_INFO("  our gripper_joint      %d: %s", (int)i, gripper_joints_[i].c_str());
00133     }
00134     action_server_.setAborted();
00135     return;
00136   }
00137 
00138   if (action_server_.isPreemptRequested())
00139   {
00140     ROS_WARN("New action goal already seems to have been cancelled!");
00141     action_server_.setPreempted();
00142     return;
00143   }
00144 
00145   // adjust all goal positions to match the given motor limits
00146   sensor_msgs::JointState adjustedJointGoal = adjustJointGoalPositionsToMotorLimits(goal->jointGoal);
00147 
00148   ROS_INFO("Sending movement to Katana arm...");
00149 
00150   for (size_t i = 0; i < adjustedJointGoal.name.size(); i++)
00151   {
00152     if (!katana_->moveJoint(katana_->getJointIndex(adjustedJointGoal.name[i]), adjustedJointGoal.position[i]))
00153     {
00154       ROS_ERROR("Problem while transferring movement to Katana arm. Aborting...");
00155       action_server_.setAborted();
00156       return;
00157     }
00158   }
00159 
00160   ros::Rate goalWait(10.0);
00161 
00162   while (ros::ok())
00163   {
00164     // always have to call this before calling someMotorCrashed() or allJointsReady()
00165     katana_->refreshMotorStatus();
00166 
00167     if (katana_->someMotorCrashed())
00168     {
00169       ROS_ERROR("Some motor has crashed! Aborting...");
00170       action_server_.setAborted();
00171       return;
00172     }
00173 
00174     if (katana_->allJointsReady())
00175     {
00176       ROS_INFO("...movement successfully executed.");
00177       action_server_.setSucceeded();
00178       return;
00179     }
00180 
00181     if (action_server_.isPreemptRequested())
00182     {
00183       ROS_WARN("Goal canceled by client while waiting for movement to finish, aborting!");
00184       action_server_.setPreempted();
00185       return;
00186     }
00187 
00188     goalWait.sleep();
00189   }
00190 }
00191 
00192 }
00193 


katana
Author(s): Martin Günther
autogenerated on Thu Aug 27 2015 13:40:35