action_server.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018 Jarek Potiuk (low bandwidth trajectory follower)
3  *
4  * Copyright 2017, 2018 Simon Rasmussen (refactor)
5  *
6  * Copyright 2015, 2016 Thomas Timm Andersen (original version)
7  *
8  * Licensed under the Apache License, Version 2.0 (the "License");
9  * you may not use this file except in compliance with the License.
10  * You may obtain a copy of the License at
11  *
12  * http://www.apache.org/licenses/LICENSE-2.0
13  *
14  * Unless required by applicable law or agreed to in writing, software
15  * distributed under the License is distributed on an "AS IS" BASIS,
16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  * See the License for the specific language governing permissions and
18  * limitations under the License.
19  */
20 
22 #include <cmath>
23 
24 ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names,
25  double max_velocity)
26  : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
27  boost::bind(&ActionServer::onCancel, this, _1), false)
28  , joint_names_(joint_names)
29  , joint_set_(joint_names.begin(), joint_names.end())
30  , max_velocity_(max_velocity)
31  , interrupt_traj_(false)
32  , has_goal_(false)
33  , running_(false)
34  , follower_(follower)
35  , state_(RobotState::Error)
36 {
37 }
38 
40 {
41  if (running_)
42  return;
43 
44  LOG_INFO("Starting ActionServer");
45  running_ = true;
47  as_.start();
48 }
49 
51 {
52  state_ = state;
53 
54  // don't interrupt if everything is fine
55  if (state == RobotState::Running)
56  return;
57 
58  // don't retry interrupts
59  if (interrupt_traj_ || !has_goal_)
60  return;
61 
62  // on successful lock we're not executing a goal so don't interrupt
63  if (tj_mutex_.try_lock())
64  {
65  tj_mutex_.unlock();
66  return;
67  }
68 
69  interrupt_traj_ = true;
70  // wait for goal to be interrupted and automagically unlock when going out of scope
71  std::lock_guard<std::mutex> lock(tj_mutex_);
72 
73  Result res;
74  res.error_code = -100;
75  res.error_string = "Robot safety stop";
76  curr_gh_.setAborted(res, res.error_string);
77 }
78 
80 {
81  q_actual_ = data.q_actual;
82  qd_actual_ = data.qd_actual;
83  return true;
84 }
85 
87 {
88  return updateState(state);
89 }
91 {
92  return updateState(state);
93 }
95 {
96  return updateState(state);
97 }
99 {
100  return updateState(state);
101 }
102 
104 {
105  Result res;
106  res.error_code = -100;
107 
108  LOG_INFO("Received new goal");
109 
110  if (!validate(gh, res) || !try_execute(gh, res))
111  {
112  LOG_WARN("Goal error: %s", res.error_string.c_str());
113  gh.setRejected(res, res.error_string);
114  }
115 }
116 
118 {
119  interrupt_traj_ = true;
120  // wait for goal to be interrupted
121  std::lock_guard<std::mutex> lock(tj_mutex_);
122 
123  Result res;
124  res.error_code = -100;
125  res.error_string = "Goal cancelled by client";
126  gh.setCanceled(res);
127 }
128 
130 {
131  return validateState(gh, res) && validateJoints(gh, res) && validateTrajectory(gh, res);
132 }
133 
135 {
136  switch (state_)
137  {
139  res.error_string = "Robot is emergency stopped";
140  return false;
141 
143  res.error_string = "Robot is protective stopped";
144  return false;
145 
146  case RobotState::Error:
147  res.error_string = "Robot is not ready, check robot_mode";
148  return false;
149 
150  case RobotState::Running:
151  return true;
152 
153  default:
154  res.error_string = "Undefined state";
155  return false;
156  }
157 }
158 
160 {
161  auto goal = gh.getGoal();
162  auto const& joints = goal->trajectory.joint_names;
163  std::set<std::string> goal_joints(joints.begin(), joints.end());
164 
165  if (goal_joints == joint_set_)
166  return true;
167 
168  res.error_code = Result::INVALID_JOINTS;
169  res.error_string = "Invalid joint names for goal\n";
170  res.error_string += "Expected: ";
171  std::for_each(goal_joints.begin(), goal_joints.end(),
172  [&res](std::string joint) { res.error_string += joint + ", "; });
173  res.error_string += "\nFound: ";
174  std::for_each(joint_set_.begin(), joint_set_.end(), [&res](std::string joint) { res.error_string += joint + ", "; });
175  return false;
176 }
177 
179 {
180  auto goal = gh.getGoal();
181  res.error_code = Result::INVALID_GOAL;
182 
183  // must at least have one point
184  if (goal->trajectory.points.size() < 1)
185  return false;
186 
187  for (auto const& point : goal->trajectory.points)
188  {
189  if (point.velocities.size() != joint_names_.size())
190  {
191  res.error_code = Result::INVALID_GOAL;
192  res.error_string = "Received a goal with an invalid number of velocities";
193  return false;
194  }
195 
196  if (point.positions.size() != joint_names_.size())
197  {
198  res.error_code = Result::INVALID_GOAL;
199  res.error_string = "Received a goal with an invalid number of positions";
200  return false;
201  }
202 
203  for (auto const& velocity : point.velocities)
204  {
205  if (!std::isfinite(velocity))
206  {
207  res.error_string = "Received a goal with infinities or NaNs in velocity";
208  return false;
209  }
210  if (std::fabs(velocity) > max_velocity_)
211  {
212  res.error_string =
213  "Received a goal with velocities that are higher than max_velocity_ " + std::to_string(max_velocity_);
214  return false;
215  }
216  }
217  for (auto const& position : point.positions)
218  {
219  if (!std::isfinite(position))
220  {
221  res.error_string = "Received a goal with infinities or NaNs in positions";
222  return false;
223  }
224  }
225  }
226 
227  // todo validate start position?
228 
229  return true;
230 }
231 
232 inline std::chrono::microseconds convert(const ros::Duration& dur)
233 {
234  return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::seconds(dur.sec) +
235  std::chrono::nanoseconds(dur.nsec));
236 }
237 
239 {
240  if (!running_)
241  {
242  res.error_string = "Internal error";
243  return false;
244  }
245  if (!tj_mutex_.try_lock())
246  {
247  interrupt_traj_ = true;
248  res.error_string = "Received another trajectory";
249  curr_gh_.setAborted(res, res.error_string);
250  tj_mutex_.lock();
251  // todo: make configurable
252  std::this_thread::sleep_for(std::chrono::milliseconds(250));
253  }
254  // locked here
255  curr_gh_ = gh;
256  interrupt_traj_ = false;
257  has_goal_ = true;
258  tj_mutex_.unlock();
259  tj_cv_.notify_one();
260  return true;
261 }
262 
263 std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joints)
264 {
265  std::vector<size_t> indecies;
266  for (auto const& aj : joint_names_)
267  {
268  size_t j = 0;
269  for (auto const& gj : goal_joints)
270  {
271  if (aj == gj)
272  break;
273  j++;
274  }
275  indecies.push_back(j);
276  }
277  return indecies;
278 }
279 
281 {
282  LOG_INFO("Trajectory thread started");
283 
284  while (running_)
285  {
286  std::unique_lock<std::mutex> lk(tj_mutex_);
287  if (!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&] { return running_ && has_goal_; }))
288  continue;
289 
290  LOG_INFO("Trajectory received and accepted");
292 
293  auto goal = curr_gh_.getGoal();
294  std::vector<TrajectoryPoint> trajectory;
295  trajectory.reserve(goal->trajectory.points.size() + 1);
296 
297  // joint names of the goal might have a different ordering compared
298  // to what URScript expects so need to map between the two
299  auto mapping = reorderMap(goal->trajectory.joint_names);
300 
301  LOG_INFO("Translating trajectory");
302 
303  auto const& fp = goal->trajectory.points[0];
304  auto fpt = convert(fp.time_from_start);
305 
306  // make sure we have a proper t0 position
307  if (fpt > std::chrono::microseconds(0))
308  {
309  LOG_INFO("Trajectory without t0 recieved, inserting t0 at currrent position");
310  trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0)));
311  }
312 
313  for (auto const& point : goal->trajectory.points)
314  {
315  std::array<double, 6> pos, vel;
316  for (size_t i = 0; i < 6; i++)
317  {
318  size_t idx = mapping[i];
319  pos[idx] = point.positions[i];
320  vel[idx] = point.velocities[i];
321  }
322  auto t = convert(point.time_from_start);
323  trajectory.push_back(TrajectoryPoint(pos, vel, t));
324  }
325 
326  double t =
327  std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size() - 1].time_from_start)
328  .count();
329  LOG_INFO("Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t);
330 
331  Result res;
332 
333  LOG_INFO("Attempting to start follower %p", &follower_);
334  if (follower_.start())
335  {
336  if (follower_.execute(trajectory, interrupt_traj_))
337  {
338  // interrupted goals must be handled by interrupt trigger
339  if (!interrupt_traj_)
340  {
341  LOG_INFO("Trajectory executed successfully");
342  res.error_code = Result::SUCCESSFUL;
343  curr_gh_.setSucceeded(res);
344  }
345  else
346  LOG_INFO("Trajectory interrupted");
347  }
348  else
349  {
350  LOG_INFO("Trajectory failed");
351  res.error_code = -100;
352  res.error_string = "Connection to robot was lost";
353  curr_gh_.setAborted(res, res.error_string);
354  }
355 
356  follower_.stop();
357  }
358  else
359  {
360  LOG_ERROR("Failed to start trajectory follower!");
361  res.error_code = -100;
362  res.error_string = "Robot connection could not be established";
363  curr_gh_.setAborted(res, res.error_string);
364  }
365 
366  has_goal_ = false;
367  lk.unlock();
368  }
369 }
virtual bool execute(std::vector< TrajectoryPoint > &trajectory, std::atomic< bool > &interrupt)=0
Error
std::condition_variable tj_cv_
Definition: action_server.h:59
RobotState state_
Definition: action_server.h:64
bool validateState(GoalHandle &gh, Result &res)
bool validateJoints(GoalHandle &gh, Result &res)
control_msgs::FollowJointTrajectoryResult Result
Definition: action_server.h:44
std::atomic< bool > running_
Definition: action_server.h:57
virtual bool consume(RTState_V1_6__7 &state)
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
std::atomic< bool > has_goal_
Definition: action_server.h:57
boost::shared_ptr< const Goal > getGoal() const
std::array< double, 6 > qd_actual
Definition: rt_state.h:50
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
GoalHandle curr_gh_
Definition: action_server.h:55
void onGoal(GoalHandle gh)
void setAccepted(const std::string &text=std::string(""))
bool validate(GoalHandle &gh, Result &res)
std::array< double, 6 > qd_actual_
Definition: action_server.h:65
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
double max_velocity_
Definition: action_server.h:53
virtual void onRobotStateChange(RobotState state)
std::array< double, 6 > q_actual
Definition: rt_state.h:49
bool updateState(RTShared &data)
#define LOG_INFO(format,...)
Definition: log.h:35
ActionServer(ActionTrajectoryFollowerInterface &follower, std::vector< std::string > &joint_names, double max_velocity)
std::mutex tj_mutex_
Definition: action_server.h:58
std::vector< unsigned int > mapping(const T &t1, const T &t2)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
std::vector< std::string > joint_names_
Definition: action_server.h:51
RobotState
bool try_execute(GoalHandle &gh, Result &res)
void trajectoryThread()
std::thread tj_thread_
Definition: action_server.h:60
bool validateTrajectory(GoalHandle &gh, Result &res)
std::set< std::string > joint_set_
Definition: action_server.h:52
std::chrono::microseconds convert(const ros::Duration &dur)
std::vector< size_t > reorderMap(std::vector< std::string > goal_joints)
#define LOG_ERROR(format,...)
Definition: log.h:36
std::atomic< bool > interrupt_traj_
Definition: action_server.h:56
void onCancel(GoalHandle gh)
ActionTrajectoryFollowerInterface & follower_
Definition: action_server.h:62
#define LOG_WARN(format,...)
Definition: log.h:34
std::array< double, 6 > q_actual_
Definition: action_server.h:65


ur_modern_driver
Author(s): Thomas Timm Andersen, Simon Rasmussen
autogenerated on Fri Jun 26 2020 03:37:00