move_joint_group.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 
00038 #include "play_motion/move_joint_group.h"
00039 
00040 #include <ros/ros.h>
00041 #include <boost/foreach.hpp>
00042 
00043 #define foreach BOOST_FOREACH
00044 
00045 namespace play_motion
00046 {
00047   MoveJointGroup::MoveJointGroup(const std::string& controller_name, const JointNames& joint_names)
00048     : busy_(false),
00049       controller_name_(controller_name),
00050       joint_names_(joint_names),
00051       client_(controller_name_ + "/follow_joint_trajectory", false)
00052   { }
00053 
00054   void MoveJointGroup::alCallback()
00055   {
00056     busy_ = false;
00057     ActionResultPtr r = client_.getResult();
00058     active_cb_(r->error_code);
00059     active_cb_.clear();
00060   }
00061 
00062   bool MoveJointGroup::isIdle() const
00063   {
00064     return !busy_;
00065   }
00066 
00067   void MoveJointGroup::cancel()
00068   {
00069     busy_ = false;
00070     client_.cancelAllGoals();
00071   }
00072 
00073   void MoveJointGroup::setCallback(const Callback& cb)
00074   {
00075     active_cb_ = cb;
00076   }
00077 
00078   const JointNames& MoveJointGroup::getJointNames() const
00079   {
00080     return joint_names_;
00081   }
00082 
00083   actionlib::SimpleClientGoalState MoveJointGroup::getState()
00084   {
00085     return client_.getState();
00086   }
00087 
00088   const std::string& MoveJointGroup::getName() const
00089   {
00090     return controller_name_;
00091   }
00092 
00093   bool MoveJointGroup::isControllingJoint(const std::string& joint_name)
00094   {
00095     if (!client_.isServerConnected())
00096       return false;
00097 
00098     foreach (const std::string& jn, joint_names_)
00099       if (joint_name == jn)
00100         return true;
00101 
00102     return false;
00103   }
00104 
00105   bool MoveJointGroup::sendGoal(const std::vector<TrajPoint>& traj)
00106   {
00107     ROS_DEBUG_STREAM("Sending trajectory goal to " << controller_name_ << ".");
00108 
00109     ActionGoal goal;
00110     goal.trajectory.joint_names = joint_names_;
00111     goal.trajectory.points.reserve(traj.size());
00112 
00113     foreach (const TrajPoint& p, traj)
00114     {
00115       if (p.positions.size() != joint_names_.size())
00116       {
00117         ROS_ERROR_STREAM("Pose size mismatch. Expected: " << joint_names_.size()
00118                          << ", got: " << p.positions.size() << ".");
00119         return false;
00120       }
00121       trajectory_msgs::JointTrajectoryPoint point;
00122 
00123       point.positions     = p.positions;         // Reach these joint positions...
00124       point.velocities    = p.velocities;        // ...with a given velocity (may be unset)
00125       point.accelerations = p.accelerations;     // ...and acceleration      (may be unset)
00126 
00127       point.time_from_start = p.time_from_start; // ...in this time
00128 
00129       goal.trajectory.points.push_back(point);
00130     }
00131     client_.sendGoal(goal, boost::bind(&MoveJointGroup::alCallback, this));
00132     busy_ = true;
00133 
00134     return true;
00135   }
00136 }


play_motion
Author(s): Paul Mathieu , Adolfo Rodriguez Tsouroukdissian
autogenerated on Sat Jun 8 2019 20:26:22