joint_trajectory_action_controller.cpp
Go to the documentation of this file.
00001 /*
00002     Copyright (c) 2011, Antons Rebguns <email>
00003     All rights reserved.
00004 
00005     Redistribution and use in source and binary forms, with or without
00006     modification, are permitted provided that the following conditions are met:
00007         * Redistributions of source code must retain the above copyright
00008         notice, this list of conditions and the following disclaimer.
00009         * Redistributions in binary form must reproduce the above copyright
00010         notice, this list of conditions and the following disclaimer in the
00011         documentation and/or other materials provided with the distribution.
00012         * Neither the name of the <organization> nor the
00013         names of its contributors may be used to endorse or promote products
00014         derived from this software without specific prior written permission.
00015 
00016     THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
00017     EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018     WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019     DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
00020     DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021     (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022     LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023     ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024     (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025     SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <sstream>
00029 #include <string>
00030 #include <vector>
00031 #include <XmlRpcValue.h>
00032 
00033 #include <dynamixel_hardware_interface/dynamixel_const.h>
00034 #include <dynamixel_hardware_interface/dynamixel_io.h>
00035 
00036 #include <dynamixel_hardware_interface/single_joint_controller.h>
00037 #include <dynamixel_hardware_interface/multi_joint_controller.h>
00038 #include <dynamixel_hardware_interface/joint_trajectory_action_controller.h>
00039 #include <dynamixel_hardware_interface/JointState.h>
00040 
00041 #include <ros/ros.h>
00042 #include <pluginlib/class_list_macros.h>
00043 #include <trajectory_msgs/JointTrajectory.h>
00044 #include <control_msgs/FollowJointTrajectoryAction.h>
00045 
00046 PLUGINLIB_DECLARE_CLASS(dynamixel_hardware_interface,
00047                         JointTrajectoryActionController,
00048                         controller::JointTrajectoryActionController,
00049                         controller::MultiJointController)
00050 
00051 namespace controller
00052 {
00053 
00054 JointTrajectoryActionController::JointTrajectoryActionController()
00055 {
00056     terminate_ = false;
00057 }
00058 
00059 JointTrajectoryActionController::~JointTrajectoryActionController()
00060 {
00061 }
00062 
00063 bool JointTrajectoryActionController::initialize(std::string name, std::vector<SingleJointController*> deps)
00064 {
00065     if (!MultiJointController::initialize(name, deps)) { return false; }
00066 
00067     update_rate_ = 1000;
00068     state_update_rate_ = 50;
00069     
00070     std::string prefix = "joint_trajectory_action_node/constraints/";
00071     
00072     c_nh_.param<double>(prefix + "goal_time", goal_time_constraint_, 0.0);
00073     c_nh_.param<double>(prefix + "stopped_velocity_tolerance", stopped_velocity_tolerance_, 0.01);
00074     c_nh_.param<double>("joint_trajectory_action_node/min_velocity", min_velocity_, 0.1);
00075     
00076     goal_constraints_.resize(num_joints_);
00077     trajectory_constraints_.resize(num_joints_);
00078     
00079     for (size_t i = 0; i < num_joints_; ++i)
00080     {
00081         c_nh_.param<double>(prefix + joint_names_[i] + "/goal", goal_constraints_[i], -1.0);
00082         c_nh_.param<double>(prefix + joint_names_[i] + "/trajectory", trajectory_constraints_[i], -1.0);
00083     }
00084     
00085     msg_.joint_names = joint_names_;
00086     msg_.desired.positions.resize(num_joints_);
00087     msg_.desired.velocities.resize(num_joints_);
00088     msg_.desired.accelerations.resize(num_joints_);
00089     msg_.actual.positions.resize(num_joints_);
00090     msg_.actual.velocities.resize(num_joints_);
00091     msg_.actual.accelerations.resize(num_joints_);
00092     msg_.error.positions.resize(num_joints_);
00093     msg_.error.velocities.resize(num_joints_);
00094     msg_.error.accelerations.resize(num_joints_);
00095     
00096     return true;
00097 }
00098 
00099 void JointTrajectoryActionController::start()
00100 {
00101     command_sub_ = c_nh_.subscribe("command", 50, &JointTrajectoryActionController::processCommand, this);
00102     state_pub_ = c_nh_.advertise<control_msgs::FollowJointTrajectoryFeedback>("state", 50);
00103     
00104     action_server_.reset(new FJTAS(c_nh_, "follow_joint_trajectory",
00105                                    boost::bind(&JointTrajectoryActionController::processFollowTrajectory, this, _1),
00106                                    false));
00107     action_server_->start();
00108     feedback_thread_ = new boost::thread(boost::bind(&JointTrajectoryActionController::updateState, this));
00109 }
00110 
00111 void JointTrajectoryActionController::stop()
00112 {
00113     {
00114         boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00115         terminate_ = true;
00116     }
00117     
00118     feedback_thread_->join();
00119     delete feedback_thread_;
00120     
00121     command_sub_.shutdown();
00122     state_pub_.shutdown();
00123     action_server_->shutdown();
00124 }
00125 
00126 void JointTrajectoryActionController::processCommand(const trajectory_msgs::JointTrajectoryConstPtr& msg)
00127 {
00128     if (action_server_->isActive()) { action_server_->setPreempted(); }
00129     
00130     while (action_server_->isActive())
00131     {
00132         ros::Duration(0.01).sleep();
00133     }
00134         
00135     processTrajectory(*msg, false);
00136 }
00137 
00138 void JointTrajectoryActionController::processFollowTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal)
00139 {
00140     processTrajectory(goal->trajectory, true);
00141 }
00142 
00143 void JointTrajectoryActionController::updateState()
00144 {
00145     ros::Rate rate(state_update_rate_);
00146     
00147     while (nh_.ok())
00148     {
00149         {
00150             boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00151             if (terminate_) { break; }
00152         }
00153         
00154         msg_.header.stamp = ros::Time::now();
00155         
00156         for (size_t j = 0; j < joint_names_.size(); ++j)
00157         {
00158             const dynamixel_hardware_interface::JointState* state = joint_states_[joint_names_[j]];
00159             msg_.desired.positions[j] = state->target_position;
00160             msg_.desired.velocities[j] = std::abs(state->target_velocity);
00161             msg_.actual.positions[j] = state->position;
00162             msg_.actual.velocities[j] = std::abs(state->velocity);
00163             msg_.error.positions[j] = msg_.actual.positions[j] - msg_.desired.positions[j];
00164             msg_.error.velocities[j] = msg_.actual.velocities[j] - msg_.desired.velocities[j];
00165         }
00166 
00167         state_pub_.publish(msg_);
00168         rate.sleep();
00169     }
00170 }
00171 
00172 void JointTrajectoryActionController::processTrajectory(const trajectory_msgs::JointTrajectory& traj, bool is_action)
00173 {
00174     control_msgs::FollowJointTrajectoryResult res;
00175     std::string error_msg;
00176     
00177     int num_points = traj.points.size();
00178     
00179     ROS_DEBUG("Received trajectory with %d points", num_points);
00180     
00181     // Maps from an index in joints_ to an index in the msg
00182     std::vector<int> lookup(num_joints_, -1);
00183     
00184     for (size_t j = 0; j < num_joints_; ++j)
00185     {
00186         for (size_t k = 0; k < traj.joint_names.size(); ++k)
00187         {
00188             if (traj.joint_names[k] == joint_names_[j])
00189             {
00190                 lookup[j] = k;
00191                 break;
00192             }
00193         }
00194 
00195         if (lookup[j] == -1)
00196         {
00197             res.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
00198             error_msg = "Unable to locate joint " + joint_names_[j] + " in the commanded trajectory";
00199             ROS_ERROR("%s", error_msg.c_str());
00200             if (is_action) { action_server_->setAborted(res, error_msg.c_str()); }
00201             return;
00202         }
00203     }
00204     
00205     std::vector<double> durations;//(num_points, 0.0);
00206     durations.resize(num_points);
00207     
00208     // find out the duration of each segment in the trajectory
00209     durations[0] = traj.points[0].time_from_start.toSec();
00210     double trajectory_duration = durations[0];
00211     
00212     for (int i = 1; i < num_points; ++i)
00213     {
00214         durations[i] = (traj.points[i].time_from_start - traj.points[i-1].time_from_start).toSec();
00215         trajectory_duration += durations[i];
00216         //ROS_DEBUG("tpi: %f, tpi-1: %f", traj.points[i].time_from_start.toSec(), traj.points[i-1].time_from_start.toSec());
00217         //ROS_DEBUG("i: %d, duration: %f, total: %f", i, durations[i], trajectory_duration);
00218     }
00219     
00220     if (traj.points[0].positions.empty())
00221     {
00222         res.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
00223         error_msg = "First point of trajectory has no positions";
00224         ROS_ERROR("%s", error_msg.c_str());
00225         if (is_action) { action_server_->setAborted(res, error_msg); }
00226         return;
00227     }
00228     
00229     std::vector<Segment> trajectory;
00230     ros::Time time = ros::Time::now() + ros::Duration(0.01);
00231 
00232     for (int i = 0; i < num_points; ++i)
00233     {
00234         const trajectory_msgs::JointTrajectoryPoint point = traj.points[i];
00235         Segment seg;
00236         
00237         if (traj.header.stamp == ros::Time(0.0))
00238         {
00239             seg.start_time = (time + point.time_from_start).toSec() - durations[i];
00240         }
00241         else
00242         {
00243             seg.start_time = (traj.header.stamp + point.time_from_start).toSec() - durations[i];
00244         }
00245         
00246         seg.duration = durations[i];
00247 
00248         // Checks that the incoming segment has the right number of elements.
00249         if (!point.velocities.empty() && point.velocities.size() != num_joints_)
00250         {
00251             res.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
00252             error_msg = "Command point " + boost::lexical_cast<std::string>(i) + " has " + boost::lexical_cast<std::string>(point.velocities.size()) + " elements for the velocities";
00253             ROS_ERROR("%s", error_msg.c_str());
00254             if (is_action) { action_server_->setAborted(res, error_msg); }
00255             return;
00256         }
00257 
00258         if (!point.positions.empty() && point.positions.size() != num_joints_)
00259         {
00260             res.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
00261             error_msg = "Command point " + boost::lexical_cast<std::string>(i) + " has " + boost::lexical_cast<std::string>(point.positions.size()) + " elements for the positions";
00262             ROS_ERROR("%s", error_msg.c_str());
00263             if (is_action) { action_server_->setAborted(res, error_msg); }
00264             return;
00265         }
00266         
00267         seg.velocities.resize(num_joints_);
00268         seg.positions.resize(num_joints_);
00269         
00270         for (size_t j = 0; j < num_joints_; ++j)
00271         {
00272             seg.velocities[j] = point.velocities[lookup[j]];
00273             seg.positions[j] = point.positions[lookup[j]];
00274         }
00275         
00276         trajectory.push_back(seg);
00277     }
00278     
00279     ROS_INFO("Trajectory start requested at %.3lf, waiting...", traj.header.stamp.toSec());
00280     ros::Time::sleepUntil(traj.header.stamp);
00281     
00282     ros::Time end_time = traj.header.stamp + ros::Duration(trajectory_duration);
00283     std::vector<ros::Time> seg_end_times(num_points, ros::Time(0.0));
00284     
00285     for (int i = 0; i < num_points; ++i)
00286     {
00287         seg_end_times[i] = ros::Time(trajectory[i].start_time + durations[i]);
00288     }
00289     
00290     ROS_INFO("Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf", time.toSec(), end_time.toSec(), trajectory_duration);
00291 
00292     trajectory_ = trajectory;
00293     ros::Time traj_start_time = ros::Time::now();
00294     ros::Rate rate(update_rate_);
00295 
00296     for (int seg = 0; seg < num_points; ++seg)
00297     {
00298         ROS_DEBUG("Processing segment %d", seg);
00299         
00300         // first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
00301         if (durations[seg] == 0.0)
00302         {
00303             ROS_DEBUG("Skipping segment %d becuase duration is 0", seg);
00304             continue;
00305         }
00306         
00307         std::map<std::string, std::vector<std::vector<int> > > multi_packet;
00308         
00309         std::map<std::string, std::vector<std::string> >::const_iterator pjit;
00310         std::vector<std::string>::const_iterator jit;
00311         
00312         for (pjit = port_to_joints_.begin(); pjit != port_to_joints_.end(); ++pjit)
00313         {
00314             std::vector<std::vector<int> > vals;
00315             
00316             for (jit = pjit->second.begin(); jit != pjit->second.end(); ++jit)
00317             {
00318                 std::string joint = *jit;
00319                 int j = joint_to_idx_[joint];
00320                 
00321                 double start_position = joint_states_[joint]->position;
00322                 if (seg != 0) { start_position = trajectory[seg-1].positions[j]; }
00323                 
00324                 double desired_position = trajectory[seg].positions[j];
00325                 double desired_velocity = std::max<double>(min_velocity_, std::abs(desired_position - start_position) / durations[seg]);
00326                 
00327                 ROS_DEBUG("\tstart_position: %f, duration: %f", start_position, durations[seg]);
00328                 ROS_DEBUG("\tport: %s, joint: %s, dpos: %f, dvel: %f", pjit->first.c_str(), joint.c_str(), desired_position, desired_velocity);
00329                 
00330                 std::vector<std::vector<int> > mvcs = joint_to_controller_[joint]->getRawMotorCommands(desired_position, desired_velocity);
00331                 for (size_t i = 0; i < mvcs.size(); ++i)
00332                 {
00333                     vals.push_back(mvcs[i]);
00334                 }
00335                 
00336                 multi_packet[pjit->first] = vals;
00337             }
00338         }
00339         
00340         std::map<std::string, std::vector<std::vector<int> > >::const_iterator mpit;
00341         for (mpit = multi_packet.begin(); mpit != multi_packet.end(); ++mpit)
00342         {
00343             port_to_io_[mpit->first]->setMultiPositionVelocity(mpit->second);
00344         }
00345         
00346         while (time < seg_end_times[seg])
00347         {
00348             // check if new trajectory was received, if so abort old one by setting the desired state to current state
00349             if (is_action && action_server_->isPreemptRequested())
00350             {
00351                 res.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
00352                 error_msg = "New trajectory received. Aborting old trajectory.";
00353                 
00354                 std::map<std::string, std::vector<std::vector<int> > > multi_packet;
00355 
00356                 std::map<std::string, std::vector<std::string> >::const_iterator pjit;
00357                 std::vector<std::string>::const_iterator jit;
00358                 
00359                 for (pjit = port_to_joints_.begin(); pjit != port_to_joints_.end(); ++pjit)
00360                 {
00361                     std::vector<std::vector<int> > vals;
00362                     
00363                     for (jit = pjit->second.begin(); jit != pjit->second.end(); ++jit)
00364                     {
00365                         std::string joint = *jit;
00366                         
00367                         double desired_position = joint_states_[joint]->position;
00368                         double desired_velocity = joint_states_[joint]->velocity;
00369                         
00370                         std::vector<std::vector<int> > mvcs = joint_to_controller_[joint]->getRawMotorCommands(desired_position, desired_velocity);
00371                         for (size_t i = 0; i < mvcs.size(); ++i)
00372                         {
00373                             vals.push_back(mvcs[i]);
00374                         }
00375                         
00376                         multi_packet[pjit->first] = vals;
00377                     }
00378                 }
00379                 
00380                 std::map<std::string, std::vector<std::vector<int> > >::const_iterator mpit;
00381                 for (mpit = multi_packet.begin(); mpit != multi_packet.end(); ++mpit)
00382                 {
00383                     port_to_io_[mpit->first]->setMultiPositionVelocity(mpit->second);
00384                 }
00385                 
00386                 action_server_->setPreempted(res, error_msg);
00387                 ROS_WARN("%s", error_msg.c_str());
00388                 return;
00389             }
00390             
00391             rate.sleep();
00392             time = ros::Time::now();
00393         }
00394         
00395         // Verifies trajectory constraints
00396         for (size_t j = 0; j < joint_names_.size(); ++j)
00397         {
00398             if (trajectory_constraints_[j] > 0.0 && msg_.error.positions[j] > trajectory_constraints_[j])
00399             {
00400                 res.error_code = control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
00401                 error_msg = "Unsatisfied position constraint for " + joint_names_[j] +
00402                             " trajectory point " + boost::lexical_cast<std::string>(seg) +
00403                             ", " + boost::lexical_cast<std::string>(msg_.error.positions[j]) +
00404                             " is larger than " + boost::lexical_cast<std::string>(trajectory_constraints_[j]);
00405                 ROS_ERROR("%s", error_msg.c_str());
00406                 if (is_action) { action_server_->setAborted(res, error_msg); }
00407                 return;
00408             }
00409         }
00410     }
00411     
00412     // let motors roll for specified amount of time
00413     ros::Duration(goal_time_constraint_).sleep();
00414     
00415     for (size_t i = 0; i < num_joints_; ++i)
00416     {
00417         if (goal_constraints_[i] > 0 && std::abs(msg_.error.positions[i]) > goal_constraints_[i])
00418         {
00419             res.error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
00420             error_msg = "Aborting because " + joint_names_[i] +
00421                         " joint wound up outside the goal constraints, " +
00422                         boost::lexical_cast<std::string>(msg_.error.positions[i]) +
00423                         " is larger than " + boost::lexical_cast<std::string>(goal_constraints_[i]);
00424             ROS_ERROR("%s", error_msg.c_str());
00425             if (is_action) { action_server_->setAborted(res, error_msg); }
00426             return;
00427         }
00428     }
00429     
00430     res.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
00431     error_msg = "Trajectory execution successfully completed";
00432     ROS_INFO("%s", error_msg.c_str());
00433     action_server_->setSucceeded(res, error_msg);
00434 }
00435 
00436 }


dynamixel_hardware_interface
Author(s): Antons Rebguns
autogenerated on Sun Oct 5 2014 23:33:10