joint_movement_action_controller.cpp
Go to the documentation of this file.
1 /*
2  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
3  * Copyright (C) 2010 University of Osnabrück
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * joint_movement_action_controller.cpp
20  *
21  * Created on: 07.04.2011
22  * Author: Henning Deeken <hdeeken@uos.de>
23  *
24  */
25 
27 #include <fstream>
28 #include <iostream>
29 #include <cstdio>
30 
31 namespace katana
32 {
33 
35  katana_(katana), action_server_(ros::NodeHandle(), "katana_arm_controller/joint_movement_action",
36  boost::bind(&JointMovementActionController::executeCB, this, _1), false)
37 {
38  joints_ = katana_->getJointNames();
39  gripper_joints_ = katana_->getGripperJointNames();
41 }
42 
44 {
45 }
46 
50 bool JointMovementActionController::suitableJointGoal(const std::vector<std::string> &jointGoalNames)
51 {
52  for (size_t i = 0; i < jointGoalNames.size(); i++)
53  {
54  bool exists = false;
55 
56  for (size_t j = 0; j < joints_.size(); j++)
57  {
58  if (jointGoalNames[i] == joints_[j])
59  exists = true;
60  }
61 
62  for (size_t k = 0; k < gripper_joints_.size(); k++)
63  {
64  if (jointGoalNames[i] == gripper_joints_[k])
65  exists = true;
66  }
67  if (!exists)
68  {
69  ROS_ERROR("joint name %s is not one of our controlled joints", jointGoalNames[i].c_str());
70  return false;
71  }
72  }
73 
74  return true;
75 }
76 
78  const sensor_msgs::JointState &jointGoal)
79 {
80  sensor_msgs::JointState adjustedJointGoal;
81 
82  adjustedJointGoal.name = jointGoal.name;
83  adjustedJointGoal.position = jointGoal.position;
84 
85  for (size_t i = 0; i < jointGoal.name.size(); i++)
86  {
87  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]);
88  }
89 
90  for (size_t i = 0; i < jointGoal.name.size(); i++)
91  {
92 
93  if (jointGoal.position[i] < katana_->getMotorLimitMin(jointGoal.name[i]))
94  {
95  adjustedJointGoal.position[i] = katana_->getMotorLimitMin(jointGoal.name[i]);
96 
97  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]));
98  }
99 
100  if (jointGoal.position[i] > katana_->getMotorLimitMax(jointGoal.name[i]))
101  {
102  adjustedJointGoal.position[i] = katana_->getMotorLimitMax(jointGoal.name[i]);
103  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]));
104  }
105  }
106 
107  return adjustedJointGoal;
108 }
109 
110 void JointMovementActionController::executeCB(const JMAS::GoalConstPtr &goal)
111 {
112  // note: the SimpleActionServer guarantees that we enter this function only when
113  // there is no other active goal. in other words, only one instance of executeCB()
114  // is ever running at the same time.
115 
116  if (!suitableJointGoal(goal->jointGoal.name))
117  {
118  ROS_ERROR("Joints on incoming goal don't match our joints/gripper_joints");
119 
120  for (size_t i = 0; i < goal->jointGoal.name.size(); i++)
121  {
122  ROS_INFO(" incoming joint %d: %s", (int)i, goal->jointGoal.name[i].c_str());
123  }
124 
125  for (size_t i = 0; i < joints_.size(); i++)
126  {
127  ROS_INFO(" our joint %d: %s", (int)i, joints_[i].c_str());
128  }
129 
130  for (size_t i = 0; i < gripper_joints_.size(); i++)
131  {
132  ROS_INFO(" our gripper_joint %d: %s", (int)i, gripper_joints_[i].c_str());
133  }
135  return;
136  }
137 
139  {
140  ROS_WARN("New action goal already seems to have been cancelled!");
142  return;
143  }
144 
145  // adjust all goal positions to match the given motor limits
146  sensor_msgs::JointState adjustedJointGoal = adjustJointGoalPositionsToMotorLimits(goal->jointGoal);
147 
148  ROS_INFO("Sending movement to Katana arm...");
149 
150  for (size_t i = 0; i < adjustedJointGoal.name.size(); i++)
151  {
152  if (!katana_->moveJoint(katana_->getJointIndex(adjustedJointGoal.name[i]), adjustedJointGoal.position[i]))
153  {
154  ROS_ERROR("Problem while transferring movement to Katana arm. Aborting...");
156  return;
157  }
158  }
159 
160  ros::Rate goalWait(10.0);
161 
162  while (ros::ok())
163  {
164  // always have to call this before calling someMotorCrashed() or allJointsReady()
165  katana_->refreshMotorStatus();
166 
167  if (katana_->someMotorCrashed())
168  {
169  ROS_ERROR("Some motor has crashed! Aborting...");
171  return;
172  }
173 
174  if (katana_->allJointsReady())
175  {
176  ROS_INFO("...movement successfully executed.");
178  return;
179  }
180 
182  {
183  ROS_WARN("Goal canceled by client while waiting for movement to finish, aborting!");
185  return;
186  }
187 
188  goalWait.sleep();
189  }
190 }
191 
192 }
193 
sensor_msgs::JointState adjustJointGoalPositionsToMotorLimits(const sensor_msgs::JointState &jointGoal)
#define ROS_WARN(...)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
bool suitableJointGoal(const std::vector< std::string > &jointGoalNames)
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
bool sleep()
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
#define ROS_ERROR(...)
JointMovementActionController(boost::shared_ptr< AbstractKatana > katana)
#define ROS_DEBUG(...)


katana
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:25