play_motion.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Modified BSD License)
00003  *
00004  *  Copyright (c) 2013, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of PAL Robotics, S.L. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #include "play_motion/play_motion.h"
00038 #include "play_motion/play_motion_helpers.h"
00039 #include <cassert>
00040 
00041 #include <boost/foreach.hpp>
00042 #include <boost/lexical_cast.hpp>
00043 
00044 #include <ros/ros.h>
00045 #include <actionlib/client/simple_action_client.h>
00046 #include <control_msgs/FollowJointTrajectoryAction.h>
00047 #include <sensor_msgs/JointState.h>
00048 
00049 #include "play_motion/approach_planner.h"
00050 #include "play_motion/move_joint_group.h"
00051 #include "play_motion/xmlrpc_helpers.h"
00052 
00053 #define foreach BOOST_FOREACH
00054 
00055 namespace
00056 {
00057   typedef play_motion::PlayMotion::GoalHandle            GoalHandle;
00058   typedef boost::shared_ptr<play_motion::MoveJointGroup> MoveJointGroupPtr;
00059   typedef std::list<MoveJointGroupPtr>                   ControllerList;
00060   typedef play_motion::PMR                               PMR;
00061   typedef actionlib::SimpleClientGoalState               SCGS;
00062 
00063   void generateErrorCode(GoalHandle goal_hdl, int error_code, SCGS ctrl_state)
00064   {
00065     typedef control_msgs::FollowJointTrajectoryResult JTR;
00066     switch (error_code)
00067     {
00068       case JTR::PATH_TOLERANCE_VIOLATED:
00069         goal_hdl->error_code = PMR::TRAJECTORY_ERROR;
00070         break;
00071       case JTR::GOAL_TOLERANCE_VIOLATED:
00072         goal_hdl->error_code = PMR::GOAL_NOT_REACHED;
00073         break;
00074       default:
00075         std::ostringstream os;
00076         goal_hdl->error_code = PMR::OTHER_ERROR;
00077         os << "Got error code " << error_code << ", motion aborted.";
00078         goal_hdl->error_string = os.str();
00079     }
00080     //TODO: add handling for controller state
00081   }
00082 
00083   void controllerCb(int error_code, GoalHandle goal_hdl, const MoveJointGroupPtr& ctrl)
00084   {
00085     ControllerList::iterator it = std::find(goal_hdl->controllers.begin(),
00086                                             goal_hdl->controllers.end(), ctrl);
00087     if (it == goal_hdl->controllers.end())
00088     {
00089       ROS_ERROR_STREAM("Something is wrong in the controller callback handling. "
00090                        << ctrl->getName() << " called a goal callback while no "
00091                        "motion goal was alive for it.");
00092       return;
00093     }
00094     goal_hdl->controllers.erase(it);
00095 
00096     ROS_DEBUG_STREAM("Return from joint group " << ctrl->getName() << ", "
00097                      << goal_hdl->controllers.size() << " active controllers, "
00098                      "error: " << error_code);
00099 
00100     if (goal_hdl->canceled)
00101     {
00102       ROS_DEBUG("The Goal was canceled, not calling Motion callback.");
00103       return;
00104     }
00105 
00106     goal_hdl->error_code = PMR::SUCCEEDED;
00107     if (error_code != 0 || ctrl->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00108     {
00109       ROS_ERROR_STREAM("Controller " << ctrl->getName() << " aborted.");
00110       goal_hdl->cancel();
00111       generateErrorCode(goal_hdl, error_code, ctrl->getState());
00112       goal_hdl->cb(goal_hdl);
00113       return;
00114     }
00115 
00116     if (goal_hdl->controllers.empty())
00117       goal_hdl->cb(goal_hdl);
00118   }
00119 
00120   template <class T>
00121   bool hasNonNullIntersection(const std::vector<T>& v1, const std::vector<T>& v2)
00122   {
00123     foreach (const T& e1, v1)
00124       foreach (const T& e2, v2)
00125         if (e1 == e2)
00126           return true;
00127     return false;
00128   }
00129 } // unnamed namespace
00130 
00131 namespace play_motion
00132 {
00133   PlayMotion::PlayMotion(ros::NodeHandle& nh) :
00134     nh_(nh),
00135     joint_states_sub_(nh_.subscribe("joint_states", 10, &PlayMotion::jointStateCb, this)),
00136     ctrlr_updater_(nh_)
00137   {
00138     ctrlr_updater_.registerUpdateCb(boost::bind(&PlayMotion::updateControllersCb, this, _1, _2));
00139 
00140     ros::NodeHandle private_nh("~");
00141     approach_planner_.reset(new ApproachPlanner(private_nh));
00142   }
00143 
00144   PlayMotion::Goal::Goal(const Callback& cbk)
00145     : error_code(0)
00146     , active_controllers(0)
00147     , cb(cbk)
00148     , canceled(false)
00149   {}
00150 
00151   void PlayMotion::Goal::cancel()
00152   {
00153     canceled = true;
00154     foreach (MoveJointGroupPtr mjg, controllers)
00155       mjg->cancel();
00156   }
00157 
00158   void PlayMotion::Goal::addController(const MoveJointGroupPtr& ctrl)
00159   {
00160     controllers.push_back(ctrl);
00161   }
00162 
00163   PlayMotion::Goal::~Goal()
00164   {
00165     cancel();
00166   }
00167 
00168   void PlayMotion::updateControllersCb(const ControllerUpdater::ControllerStates& states,
00169                                        const ControllerUpdater::ControllerJoints& joints)
00170   {
00171     typedef std::pair<std::string, ControllerUpdater::ControllerState> ctrlr_state_pair_t;
00172     move_joint_groups_.clear();
00173     foreach (const ctrlr_state_pair_t& p, states)
00174     {
00175       if (p.second != ControllerUpdater::RUNNING)
00176         continue;
00177       move_joint_groups_.push_back(MoveJointGroupPtr(new MoveJointGroup(p.first, joints.at(p.first))));
00178       ROS_DEBUG_STREAM("Controller '" << p.first << "' with " << joints.at(p.first).size() << " joints.");
00179     }
00180   }
00181 
00182   void PlayMotion::jointStateCb(const sensor_msgs::JointStatePtr& msg)
00183   {
00184     joint_states_.clear();
00185     for (uint32_t i=0; i < msg->name.size(); ++i)
00186       joint_states_[msg->name[i]] = msg->position[i];
00187   }
00188 
00189   bool PlayMotion::getGroupTraj(MoveJointGroupPtr move_joint_group,
00190                                 const JointNames& motion_joints,
00191                                 const Trajectory& motion_points, Trajectory& traj_group)
00192   {
00193     JointNames                 group_joint_names = move_joint_group->getJointNames();
00194     std::vector<double>        joint_states;
00195     std::map<std::string, int> joint_index;
00196 
00197     traj_group.clear();
00198     traj_group.reserve(motion_points.size());
00199 
00200     foreach (const std::string& jn, group_joint_names)
00201     {
00202       // store the index of this joint in the given motion
00203       int index = std::find(motion_joints.begin(), motion_joints.end(), jn) - motion_joints.begin();
00204       joint_index[jn] = index;
00205 
00206       // retrieve joint state,  we should have it from the joint_states subscriber
00207       if (joint_states_.find(jn) == joint_states_.end())
00208       {
00209         ROS_ERROR_STREAM("Could not get current position of joint \'" << jn << "\'.");
00210         return false;
00211       }
00212       joint_states.push_back(joint_states_[jn]);
00213     }
00214 
00215     foreach (const TrajPoint& p, motion_points)
00216     {
00217       bool has_velocities    = !p.velocities.empty();
00218       bool has_accelerations = !p.accelerations.empty();
00219       TrajPoint point;
00220       point.positions.resize(group_joint_names.size());
00221       if (has_velocities)
00222         point.velocities.resize(group_joint_names.size(), 0);
00223       if (has_accelerations)
00224         point.accelerations.resize(group_joint_names.size(), 0);
00225       point.time_from_start = p.time_from_start;
00226 
00227       for (std::size_t i = 0; i < group_joint_names.size(); ++i)
00228       {
00229         // first assignment, overriden by given motion if joint specified
00230         point.positions[i] = joint_states[i];
00231 
00232         size_t index = joint_index[group_joint_names[i]];
00233         if (index < motion_joints.size())
00234         {
00235           point.positions[i] = p.positions[index];
00236           if (has_velocities)
00237             point.velocities[i] = p.velocities[index];
00238           if (has_accelerations)
00239             point.accelerations[i] = p.accelerations[index];
00240         }
00241       }
00242       traj_group.push_back(point);
00243     }
00244     return true;
00245   }
00246 
00247   void PlayMotion::getMotionJoints(const std::string& motion_name, JointNames& motion_joints)
00248   {
00249     try
00250     {
00251       ::play_motion::getMotionJoints(ros::NodeHandle("~"), motion_name, motion_joints);
00252     }
00253     catch (const xh::XmlrpcHelperException& e)
00254     {
00255       std::ostringstream error_msg;
00256       error_msg << "Could not parse motion '" << motion_name << "': " << e.what();
00257       throw PMException(error_msg.str(), PMR::MOTION_NOT_FOUND);
00258     }
00259     catch (const ros::Exception& e)
00260     {
00261       std::ostringstream error_msg;
00262       error_msg << "could not parse motion '" << motion_name << "': " << e.what();
00263       throw PMException(error_msg.str(), PMR::MOTION_NOT_FOUND);
00264     }
00265   }
00266 
00267   void PlayMotion::getMotionPoints(const std::string& motion_name, Trajectory& motion_points)
00268   {
00269     try
00270     {
00271       ::play_motion::getMotionPoints(ros::NodeHandle("~"), motion_name, motion_points);
00272     }
00273     catch (const xh::XmlrpcHelperException& e)
00274     {
00275       std::ostringstream error_msg;
00276       error_msg << "Could not parse motion '" << motion_name << "': " << e.what();
00277       throw PMException(error_msg.str(), PMR::MOTION_NOT_FOUND);
00278     }
00279     catch (const ros::Exception& e)
00280     {
00281       std::ostringstream error_msg;
00282       error_msg << "could not parse motion '" << motion_name << "': " << e.what();
00283       throw PMException(error_msg.str(), PMR::MOTION_NOT_FOUND);
00284     }
00285   }
00286 
00287   ControllerList PlayMotion::getMotionControllers(const JointNames& motion_joints)
00288   {
00289     // Populate list of controllers containing at least one motion joint,...
00290     ControllerList ctrlr_list;
00291     foreach (MoveJointGroupPtr move_joint_group, move_joint_groups_)
00292     {
00293       if (hasNonNullIntersection(motion_joints, move_joint_group->getJointNames()))
00294         ctrlr_list.push_back(move_joint_group);
00295     }
00296 
00297     // ...check that all motion joints are contained in this list...
00298     foreach (const std::string& jn, motion_joints)
00299     {
00300       foreach (MoveJointGroupPtr ctrlr, ctrlr_list)
00301         if (ctrlr->isControllingJoint(jn))
00302           goto next_joint;
00303 
00304       throw PMException("No controller was found for joint '" + jn + "'", PMR::MISSING_CONTROLLER);
00305 next_joint:;
00306     }
00307 
00308     // ...and that no controller in the list is busy executing another goal
00309     foreach (MoveJointGroupPtr move_joint_group, ctrlr_list)
00310     {
00311       if(!move_joint_group->isIdle())
00312         throw PMException("Controller '" + move_joint_group->getName() + "' is busy", PMR::CONTROLLER_BUSY);
00313     }
00314 
00315     return ctrlr_list;
00316   }
00317 
00318   bool PlayMotion::run(const std::string& motion_name,
00319                        bool               skip_planning,
00320                        GoalHandle&        goal_hdl,
00321                        const Callback&    cb)
00322   {
00323     JointNames                              motion_joints;
00324     Trajectory                              motion_points;
00325     std::map<MoveJointGroupPtr, Trajectory> joint_group_traj;
00326 
00327     goal_hdl = GoalHandle(new Goal(cb));
00328 
00329     try
00330     {
00331       getMotionJoints(motion_name, motion_joints);
00332       ControllerList groups = getMotionControllers(motion_joints); // Checks many preconditions
00333       getMotionPoints(motion_name, motion_points);
00334 
00335       std::vector<double> curr_pos; // Current position of motion joints
00336       foreach(const std::string& motion_joint, motion_joints)
00337         curr_pos.push_back(joint_states_[motion_joint]); // TODO: What if motion joint does not exist?
00338 
00339       // Approach trajectory
00340       Trajectory motion_points_safe;
00341       if (!approach_planner_->prependApproach(motion_joints, curr_pos,
00342                                               skip_planning,
00343                                               motion_points, motion_points_safe))
00344         throw PMException("Approach motion planning failed", PMR::NO_PLAN_FOUND);// TODO: Expose descriptive error string from approach_planner
00345 
00346       // TODO: Resample and validate output trajectory
00347       populateVelocities(motion_points_safe, motion_points_safe);
00348 
00349       // Seed target pose with current joint state
00350       foreach (MoveJointGroupPtr move_joint_group, groups)
00351       {
00352         if(!getGroupTraj(move_joint_group, motion_joints, motion_points_safe,
00353                          joint_group_traj[move_joint_group]))
00354           throw PMException("Missing joint state for joint in controller '"
00355                             + move_joint_group->getName() + "'");
00356       }
00357       if (joint_group_traj.empty())
00358         throw PMException("Nothing to send to controllers");
00359 
00360       // Send pose commands
00361       typedef std::pair<MoveJointGroupPtr, Trajectory> traj_pair_t;
00362       foreach (const traj_pair_t& p, joint_group_traj)
00363       {
00364         goal_hdl->addController(p.first);
00365         p.first->setCallback(boost::bind(controllerCb, _1, goal_hdl, p.first));
00366         if (!p.first->sendGoal(p.second))
00367           throw PMException("Controller '" + p.first->getName() + "' did not accept trajectory, "
00368                             "canceling everything");
00369       }
00370     }
00371     catch (const PMException& e)
00372     {
00373       goal_hdl->error_string = e.what();
00374       goal_hdl->error_code = e.error_code();
00375       return false;
00376     }
00377     return true;
00378   }
00379 }


play_motion
Author(s): Paul Mathieu
autogenerated on Wed Aug 26 2015 15:29:25