moveit_fake_controllers.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ioan A. Sucan
00005  *  Copyright (c) 2016, Robert Haschke
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the names of the authors nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 /* Author: Ioan Sucan, Robert Haschke */
00037 
00038 #include "moveit_fake_controllers.h"
00039 #include <ros/param.h>
00040 #include <sensor_msgs/JointState.h>
00041 #include <boost/thread.hpp>
00042 #include <limits>
00043 
00044 namespace moveit_fake_controller_manager
00045 {
00046 BaseFakeController::BaseFakeController(const std::string& name, const std::vector<std::string>& joints,
00047                                        const ros::Publisher& pub)
00048   : moveit_controller_manager::MoveItControllerHandle(name), joints_(joints), pub_(pub)
00049 {
00050   std::stringstream ss;
00051   ss << "Fake controller '" << name << "' with joints [ ";
00052   std::copy(joints.begin(), joints.end(), std::ostream_iterator<std::string>(ss, " "));
00053   ss << "]";
00054   ROS_INFO_STREAM(ss.str());
00055 }
00056 
00057 void BaseFakeController::getJoints(std::vector<std::string>& joints) const
00058 {
00059   joints = joints_;
00060 }
00061 
00062 moveit_controller_manager::ExecutionStatus BaseFakeController::getLastExecutionStatus()
00063 {
00064   return moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00065 }
00066 
00067 LastPointController::LastPointController(const std::string& name, const std::vector<std::string>& joints,
00068                                          const ros::Publisher& pub)
00069   : BaseFakeController(name, joints, pub)
00070 {
00071 }
00072 
00073 LastPointController::~LastPointController()
00074 {
00075 }
00076 
00077 bool LastPointController::sendTrajectory(const moveit_msgs::RobotTrajectory& t)
00078 {
00079   ROS_INFO("Fake execution of trajectory");
00080   if (t.joint_trajectory.points.empty())
00081     return true;
00082 
00083   sensor_msgs::JointState js;
00084   const trajectory_msgs::JointTrajectoryPoint& last = t.joint_trajectory.points.back();
00085   js.header = t.joint_trajectory.header;
00086   js.header.stamp = ros::Time::now();
00087   js.name = t.joint_trajectory.joint_names;
00088   js.position = last.positions;
00089   js.velocity = last.velocities;
00090   js.effort = last.effort;
00091   pub_.publish(js);
00092 
00093   return true;
00094 }
00095 
00096 bool LastPointController::cancelExecution()
00097 {
00098   return true;
00099 }
00100 
00101 bool LastPointController::waitForExecution(const ros::Duration&)
00102 {
00103   ros::Duration(0.5).sleep();  // give some time to receive the published JointState
00104   return true;
00105 }
00106 
00107 ThreadedController::ThreadedController(const std::string& name, const std::vector<std::string>& joints,
00108                                        const ros::Publisher& pub)
00109   : BaseFakeController(name, joints, pub)
00110 {
00111 }
00112 
00113 ThreadedController::~ThreadedController()
00114 {
00115   ThreadedController::cancelTrajectory();
00116 }
00117 
00118 void ThreadedController::cancelTrajectory()
00119 {
00120   cancel_ = true;
00121   thread_.join();
00122 }
00123 
00124 bool ThreadedController::sendTrajectory(const moveit_msgs::RobotTrajectory& t)
00125 {
00126   cancelTrajectory();  // cancel any previous fake motion
00127   cancel_ = false;
00128   status_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
00129   thread_ = boost::thread(boost::bind(&ThreadedController::execTrajectory, this, t));
00130   return true;
00131 }
00132 
00133 bool ThreadedController::cancelExecution()
00134 {
00135   cancelTrajectory();
00136   ROS_INFO("Fake trajectory execution cancelled");
00137   status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00138   return true;
00139 }
00140 
00141 bool ThreadedController::waitForExecution(const ros::Duration&)
00142 {
00143   thread_.join();
00144   status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00145   return true;
00146 }
00147 
00148 moveit_controller_manager::ExecutionStatus ThreadedController::getLastExecutionStatus()
00149 {
00150   return status_;
00151 }
00152 
00153 ViaPointController::ViaPointController(const std::string& name, const std::vector<std::string>& joints,
00154                                        const ros::Publisher& pub)
00155   : ThreadedController(name, joints, pub)
00156 {
00157 }
00158 
00159 ViaPointController::~ViaPointController()
00160 {
00161 }
00162 
00163 void ViaPointController::execTrajectory(const moveit_msgs::RobotTrajectory& t)
00164 {
00165   ROS_INFO("Fake execution of trajectory");
00166   sensor_msgs::JointState js;
00167   js.header = t.joint_trajectory.header;
00168   js.name = t.joint_trajectory.joint_names;
00169 
00170   // publish joint states for all intermediate via points of the trajectory
00171   // no further interpolation
00172   ros::Time startTime = ros::Time::now();
00173   for (std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator via = t.joint_trajectory.points.begin(),
00174                                                                           end = t.joint_trajectory.points.end();
00175        !cancelled() && via != end; ++via)
00176   {
00177     js.position = via->positions;
00178     js.velocity = via->velocities;
00179     js.effort = via->effort;
00180 
00181     ros::Duration waitTime = via->time_from_start - (ros::Time::now() - startTime);
00182     if (waitTime.toSec() > std::numeric_limits<float>::epsilon())
00183     {
00184       ROS_DEBUG("Fake execution: waiting %0.1fs for next via point, %ld remaining", waitTime.toSec(), end - via);
00185       waitTime.sleep();
00186     }
00187     js.header.stamp = ros::Time::now();
00188     pub_.publish(js);
00189   }
00190   ROS_DEBUG("Fake execution of trajectory: done");
00191 }
00192 
00193 InterpolatingController::InterpolatingController(const std::string& name, const std::vector<std::string>& joints,
00194                                                  const ros::Publisher& pub)
00195   : ThreadedController(name, joints, pub), rate_(10)
00196 {
00197   double r;
00198   if (ros::param::get("~fake_interpolating_controller_rate", r))
00199     rate_ = ros::WallRate(r);
00200 }
00201 
00202 InterpolatingController::~InterpolatingController()
00203 {
00204 }
00205 
00206 namespace
00207 {
00208 void interpolate(sensor_msgs::JointState& js, const trajectory_msgs::JointTrajectoryPoint& prev,
00209                  const trajectory_msgs::JointTrajectoryPoint& next, const ros::Duration& elapsed)
00210 {
00211   double duration = (next.time_from_start - prev.time_from_start).toSec();
00212   double alpha = 1.0;
00213   if (duration > std::numeric_limits<double>::epsilon())
00214     alpha = (elapsed - prev.time_from_start).toSec() / duration;
00215 
00216   js.position.resize(prev.positions.size());
00217   for (std::size_t i = 0, end = prev.positions.size(); i < end; ++i)
00218   {
00219     js.position[i] = prev.positions[i] + alpha * (next.positions[i] - prev.positions[i]);
00220   }
00221 }
00222 }
00223 
00224 void InterpolatingController::execTrajectory(const moveit_msgs::RobotTrajectory& t)
00225 {
00226   ROS_INFO("Fake execution of trajectory");
00227   if (t.joint_trajectory.points.empty())
00228     return;
00229 
00230   sensor_msgs::JointState js;
00231   js.header = t.joint_trajectory.header;
00232   js.name = t.joint_trajectory.joint_names;
00233 
00234   const std::vector<trajectory_msgs::JointTrajectoryPoint>& points = t.joint_trajectory.points;
00235   std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator prev = points.begin(),  // previous via point
00236       next = points.begin() + 1,  // currently targetted via point
00237       end = points.end();
00238 
00239   ros::Time startTime = ros::Time::now();
00240   while (!cancelled())
00241   {
00242     ros::Duration elapsed = ros::Time::now() - startTime;
00243     // hop to next targetted via point
00244     while (next != end && elapsed > next->time_from_start)
00245     {
00246       ++prev;
00247       ++next;
00248     }
00249     if (next == end)
00250       break;
00251 
00252     double duration = (next->time_from_start - prev->time_from_start).toSec();
00253     ROS_DEBUG("elapsed: %.3f via points %td,%td / %td  alpha: %.3f", elapsed.toSec(), prev - points.begin(),
00254               next - points.begin(), end - points.begin(),
00255               duration > std::numeric_limits<double>::epsilon() ? (elapsed - prev->time_from_start).toSec() / duration :
00256                                                                   1.0);
00257     interpolate(js, *prev, *next, elapsed);
00258     js.header.stamp = ros::Time::now();
00259     pub_.publish(js);
00260     rate_.sleep();
00261   }
00262   if (cancelled())
00263     return;
00264 
00265   ros::Duration elapsed = ros::Time::now() - startTime;
00266   ROS_DEBUG("elapsed: %.3f via points %td,%td / %td  alpha: 1.0", elapsed.toSec(), prev - points.begin(),
00267             next - points.begin(), end - points.begin());
00268 
00269   // publish last point
00270   interpolate(js, *prev, *prev, prev->time_from_start);
00271   js.header.stamp = ros::Time::now();
00272   pub_.publish(js);
00273 
00274   ROS_DEBUG("Fake execution of trajectory: done");
00275 }
00276 
00277 }  // end namespace moveit_fake_controller_manager


moveit_fake_controller_manager
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:21:26