moveit_fake_controllers.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Ioan A. Sucan
5  * Copyright (c) 2016, Robert Haschke
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the names of the authors nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Ioan Sucan, Robert Haschke */
37 
39 #include <ros/param.h>
40 #include <sensor_msgs/JointState.h>
41 #include <boost/thread.hpp>
42 #include <limits>
43 
45 {
46 BaseFakeController::BaseFakeController(const std::string& name, const std::vector<std::string>& joints,
47  const ros::Publisher& pub)
48  : moveit_controller_manager::MoveItControllerHandle(name), joints_(joints), pub_(pub)
49 {
50  std::stringstream ss;
51  ss << "Fake controller '" << name << "' with joints [ ";
52  std::copy(joints.begin(), joints.end(), std::ostream_iterator<std::string>(ss, " "));
53  ss << "]";
54  ROS_INFO_STREAM(ss.str());
55 }
56 
57 void BaseFakeController::getJoints(std::vector<std::string>& joints) const
58 {
59  joints = joints_;
60 }
61 
63 {
65 }
66 
67 LastPointController::LastPointController(const std::string& name, const std::vector<std::string>& joints,
68  const ros::Publisher& pub)
69  : BaseFakeController(name, joints, pub)
70 {
71 }
72 
74 {
75 }
76 
77 bool LastPointController::sendTrajectory(const moveit_msgs::RobotTrajectory& t)
78 {
79  ROS_INFO("Fake execution of trajectory");
80  if (t.joint_trajectory.points.empty())
81  return true;
82 
83  sensor_msgs::JointState js;
84  const trajectory_msgs::JointTrajectoryPoint& last = t.joint_trajectory.points.back();
85  js.header = t.joint_trajectory.header;
86  js.header.stamp = ros::Time::now();
87  js.name = t.joint_trajectory.joint_names;
88  js.position = last.positions;
89  js.velocity = last.velocities;
90  js.effort = last.effort;
91  pub_.publish(js);
92 
93  return true;
94 }
95 
97 {
98  return true;
99 }
100 
102 {
103  ros::Duration(0.5).sleep(); // give some time to receive the published JointState
104  return true;
105 }
106 
107 ThreadedController::ThreadedController(const std::string& name, const std::vector<std::string>& joints,
108  const ros::Publisher& pub)
109  : BaseFakeController(name, joints, pub)
110 {
111 }
112 
114 {
116 }
117 
119 {
120  cancel_ = true;
121  thread_.join();
122 }
123 
124 bool ThreadedController::sendTrajectory(const moveit_msgs::RobotTrajectory& t)
125 {
126  cancelTrajectory(); // cancel any previous fake motion
127  cancel_ = false;
129  thread_ = boost::thread(boost::bind(&ThreadedController::execTrajectory, this, t));
130  return true;
131 }
132 
134 {
136  ROS_INFO("Fake trajectory execution cancelled");
138  return true;
139 }
140 
142 {
143  thread_.join();
145  return true;
146 }
147 
149 {
150  return status_;
151 }
152 
153 ViaPointController::ViaPointController(const std::string& name, const std::vector<std::string>& joints,
154  const ros::Publisher& pub)
155  : ThreadedController(name, joints, pub)
156 {
157 }
158 
160 {
161 }
162 
163 void ViaPointController::execTrajectory(const moveit_msgs::RobotTrajectory& t)
164 {
165  ROS_INFO("Fake execution of trajectory");
166  sensor_msgs::JointState js;
167  js.header = t.joint_trajectory.header;
168  js.name = t.joint_trajectory.joint_names;
169 
170  // publish joint states for all intermediate via points of the trajectory
171  // no further interpolation
172  ros::Time startTime = ros::Time::now();
173  for (std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator via = t.joint_trajectory.points.begin(),
174  end = t.joint_trajectory.points.end();
175  !cancelled() && via != end; ++via)
176  {
177  js.position = via->positions;
178  js.velocity = via->velocities;
179  js.effort = via->effort;
180 
181  ros::Duration waitTime = via->time_from_start - (ros::Time::now() - startTime);
182  if (waitTime.toSec() > std::numeric_limits<float>::epsilon())
183  {
184  ROS_DEBUG("Fake execution: waiting %0.1fs for next via point, %ld remaining", waitTime.toSec(), end - via);
185  waitTime.sleep();
186  }
187  js.header.stamp = ros::Time::now();
188  pub_.publish(js);
189  }
190  ROS_DEBUG("Fake execution of trajectory: done");
191 }
192 
193 InterpolatingController::InterpolatingController(const std::string& name, const std::vector<std::string>& joints,
194  const ros::Publisher& pub)
195  : ThreadedController(name, joints, pub), rate_(10)
196 {
197  double r;
198  if (ros::param::get("~fake_interpolating_controller_rate", r))
199  rate_ = ros::WallRate(r);
200 }
201 
203 {
204 }
205 
206 namespace
207 {
208 void interpolate(sensor_msgs::JointState& js, const trajectory_msgs::JointTrajectoryPoint& prev,
209  const trajectory_msgs::JointTrajectoryPoint& next, const ros::Duration& elapsed)
210 {
211  double duration = (next.time_from_start - prev.time_from_start).toSec();
212  double alpha = 1.0;
213  if (duration > std::numeric_limits<double>::epsilon())
214  alpha = (elapsed - prev.time_from_start).toSec() / duration;
215 
216  js.position.resize(prev.positions.size());
217  for (std::size_t i = 0, end = prev.positions.size(); i < end; ++i)
218  {
219  js.position[i] = prev.positions[i] + alpha * (next.positions[i] - prev.positions[i]);
220  }
221 }
222 }
223 
224 void InterpolatingController::execTrajectory(const moveit_msgs::RobotTrajectory& t)
225 {
226  ROS_INFO("Fake execution of trajectory");
227  if (t.joint_trajectory.points.empty())
228  return;
229 
230  sensor_msgs::JointState js;
231  js.header = t.joint_trajectory.header;
232  js.name = t.joint_trajectory.joint_names;
233 
234  const std::vector<trajectory_msgs::JointTrajectoryPoint>& points = t.joint_trajectory.points;
235  std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator prev = points.begin(), // previous via point
236  next = points.begin() + 1, // currently targetted via point
237  end = points.end();
238 
239  ros::Time startTime = ros::Time::now();
240  while (!cancelled())
241  {
242  ros::Duration elapsed = ros::Time::now() - startTime;
243  // hop to next targetted via point
244  while (next != end && elapsed > next->time_from_start)
245  {
246  ++prev;
247  ++next;
248  }
249  if (next == end)
250  break;
251 
252  double duration = (next->time_from_start - prev->time_from_start).toSec();
253  ROS_DEBUG("elapsed: %.3f via points %td,%td / %td alpha: %.3f", elapsed.toSec(), prev - points.begin(),
254  next - points.begin(), end - points.begin(),
255  duration > std::numeric_limits<double>::epsilon() ? (elapsed - prev->time_from_start).toSec() / duration :
256  1.0);
257  interpolate(js, *prev, *next, elapsed);
258  js.header.stamp = ros::Time::now();
259  pub_.publish(js);
260  rate_.sleep();
261  }
262  if (cancelled())
263  return;
264 
265  ros::Duration elapsed = ros::Time::now() - startTime;
266  ROS_DEBUG("elapsed: %.3f via points %td,%td / %td alpha: 1.0", elapsed.toSec(), prev - points.begin(),
267  next - points.begin(), end - points.begin());
268 
269  // publish last point
270  interpolate(js, *prev, *prev, prev->time_from_start);
271  js.header.stamp = ros::Time::now();
272  pub_.publish(js);
273 
274  ROS_DEBUG("Fake execution of trajectory: done");
275 }
276 
277 } // end namespace moveit_fake_controller_manager
void publish(const boost::shared_ptr< M > &message) const
BaseFakeController(const std::string &name, const std::vector< std::string > &joints, const ros::Publisher &pub)
virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus()
ViaPointController(const std::string &name, const std::vector< std::string > &joints, const ros::Publisher &pub)
virtual void execTrajectory(const moveit_msgs::RobotTrajectory &t)=0
bool sleep() const
virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &t)
void getJoints(std::vector< std::string > &joints) const
virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &t)
ThreadedController(const std::string &name, const std::vector< std::string > &joints, const ros::Publisher &pub)
ROSCPP_DECL bool get(const std::string &key, std::string &s)
#define ROS_INFO(...)
virtual void execTrajectory(const moveit_msgs::RobotTrajectory &t)
LastPointController(const std::string &name, const std::vector< std::string > &joints, const ros::Publisher &pub)
virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus()
moveit_controller_manager::ExecutionStatus status_
#define ROS_INFO_STREAM(args)
static Time now()
r
InterpolatingController(const std::string &name, const std::vector< std::string > &joints, const ros::Publisher &pub)
virtual void execTrajectory(const moveit_msgs::RobotTrajectory &t)
#define ROS_DEBUG(...)


moveit_fake_controller_manager
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:17:49