joint_trajectory_action.cpp
Go to the documentation of this file.
00001 /*
00002 * Software License Agreement (BSD License)
00003 *
00004 * Copyright (c) 2011, Southwest Research Institute
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 are met:
00009 *
00010 *       * Redistributions of source code must retain the above copyright
00011 *       notice, this list of conditions and the following disclaimer.
00012 *       * Redistributions in binary form must reproduce the above copyright
00013 *       notice, this list of conditions and the following disclaimer in the
00014 *       documentation and/or other materials provided with the distribution.
00015 *       * Neither the name of the Southwest Research Institute, nor the names
00016 *       of its contributors may be used to endorse or promote products derived
00017 *       from this software without specific prior written permission.
00018 *
00019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 * POSSIBILITY OF SUCH DAMAGE.
00030 */
00031 
00032 
00033 #include <ros/ros.h>
00034 #include <actionlib/server/action_server.h>
00035 
00036 #include <trajectory_msgs/JointTrajectory.h>
00037 #include <control_msgs/FollowJointTrajectoryAction.h>
00038 #include <control_msgs/FollowJointTrajectoryFeedback.h>
00039 
00040 const double DEFAULT_GOAL_THRESHOLD = 0.01;
00041 
00042 class JointTrajectoryExecuter
00043 {
00044 private:
00045   typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> JTAS;
00046   typedef JTAS::GoalHandle GoalHandle;
00047 public:
00048   JointTrajectoryExecuter(ros::NodeHandle &n) :
00049     node_(n),
00050     action_server_(node_, "joint_trajectory_action",
00051                    boost::bind(&JointTrajectoryExecuter::goalCB, this, _1),
00052                    boost::bind(&JointTrajectoryExecuter::cancelCB, this, _1),
00053                    false),
00054     has_active_goal_(false)
00055   {
00056     using namespace XmlRpc;
00057     ros::NodeHandle pn("~");
00058 
00059     joint_names_.push_back("joint_1");
00060     joint_names_.push_back("joint_2");
00061     joint_names_.push_back("joint_3");
00062     joint_names_.push_back("joint_4");
00063     joint_names_.push_back("joint_5");
00064     joint_names_.push_back("joint_6");
00065 
00066     pn.param("constraints/goal_time", goal_time_constraint_, 0.0);
00067 
00068     // Gets the constraints for each joint.
00069     for (size_t i = 0; i < joint_names_.size(); ++i)
00070     {
00071       std::string ns = std::string("constraints/") + joint_names_[i];
00072       double g, t;
00073       pn.param(ns + "/goal", g, DEFAULT_GOAL_THRESHOLD);
00074       pn.param(ns + "/trajectory", t, -1.0);
00075       goal_constraints_[joint_names_[i]] = g;
00076       trajectory_constraints_[joint_names_[i]] = t;
00077     }
00078     pn.param("constraints/stopped_velocity_tolerance", stopped_velocity_tolerance_, 0.01);
00079 
00080 
00081     pub_controller_command_ =
00082       node_.advertise<trajectory_msgs::JointTrajectory>("command", 1);
00083     sub_controller_state_ =
00084       node_.subscribe("feedback_states", 1, &JointTrajectoryExecuter::controllerStateCB, this);
00085     
00086     action_server_.start();
00087   }
00088 
00089   ~JointTrajectoryExecuter()
00090   {
00091    pub_controller_command_.shutdown();
00092     sub_controller_state_.shutdown();
00093     watchdog_timer_.stop();
00094   }
00095 
00096 private:
00097 
00098   static bool setsEqual(const std::vector<std::string> &a, const std::vector<std::string> &b)
00099   {
00100     if (a.size() != b.size())
00101       return false;
00102 
00103     for (size_t i = 0; i < a.size(); ++i)
00104     {
00105       if (count(b.begin(), b.end(), a[i]) != 1)
00106         return false;
00107     }
00108     for (size_t i = 0; i < b.size(); ++i)
00109     {
00110       if (count(a.begin(), a.end(), b[i]) != 1)
00111         return false;
00112     }
00113 
00114     return true;
00115   }
00116 
00117   void watchdog(const ros::TimerEvent &e)
00118   {
00119     ros::Time now = ros::Time::now();
00120 
00121     // Aborts the active goal if the controller does not appear to be active.
00122     if (has_active_goal_)
00123     {
00124       bool should_abort = false;
00125       if (!last_controller_state_)
00126       {
00127         should_abort = true;
00128         ROS_WARN("Aborting goal because we have never heard a controller state message.");
00129       }
00130       else if ((now - last_controller_state_->header.stamp) > ros::Duration(5.0))
00131       {
00132         should_abort = true;
00133         ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
00134                  (now - last_controller_state_->header.stamp).toSec());
00135       }
00136 
00137       if (should_abort)
00138       {
00139         // Stops the controller.
00140         trajectory_msgs::JointTrajectory empty;
00141         empty.joint_names = joint_names_;
00142         pub_controller_command_.publish(empty);
00143 
00144         // Marks the current goal as aborted.
00145         active_goal_.setAborted();
00146         has_active_goal_ = false;
00147       }
00148     }
00149   }
00150 
00151   void goalCB(GoalHandle gh)
00152   {
00153     // Ensures that the joints in the goal match the joints we are commanding.
00154     ROS_DEBUG("Received goal: goalCB");
00155     if (!setsEqual(joint_names_, gh.getGoal()->trajectory.joint_names))
00156     {
00157       ROS_ERROR("Joints on incoming goal don't match our joints");
00158       gh.setRejected();
00159       return;
00160     }
00161 
00162     // Cancels the currently active goal.
00163     if (has_active_goal_)
00164     {
00165       ROS_DEBUG("Received new goal, canceling current goal");
00166       // Stops the controller.
00167       trajectory_msgs::JointTrajectory empty;
00168       empty.joint_names = joint_names_;
00169       pub_controller_command_.publish(empty);
00170 
00171       // Marks the current goal as canceled.
00172       active_goal_.setCanceled();
00173       has_active_goal_ = false;
00174     }
00175 
00176     gh.setAccepted();
00177     active_goal_ = gh;
00178     has_active_goal_ = true;
00179 
00180     // Sends the trajectory along to the controller
00181     ROS_DEBUG("Publishing trajectory");
00182     current_traj_ = active_goal_.getGoal()->trajectory;
00183     pub_controller_command_.publish(current_traj_);
00184   }
00185 
00186   void cancelCB(GoalHandle gh)
00187   {
00188     ROS_DEBUG("Received action cancel request");
00189     if (active_goal_ == gh)
00190     {
00191       // Stops the controller.
00192       trajectory_msgs::JointTrajectory empty;
00193       empty.joint_names = joint_names_;
00194       pub_controller_command_.publish(empty);
00195 
00196       // Marks the current goal as canceled.
00197       active_goal_.setCanceled();
00198       has_active_goal_ = false;
00199     }
00200   }
00201 
00202 
00203   ros::NodeHandle node_;
00204   JTAS action_server_;
00205   ros::Publisher pub_controller_command_;
00206   ros::Subscriber sub_controller_state_;
00207   ros::Timer watchdog_timer_;
00208 
00209   bool has_active_goal_;
00210   GoalHandle active_goal_;
00211   trajectory_msgs::JointTrajectory current_traj_;
00212 
00213 
00214   std::vector<std::string> joint_names_;
00215   std::map<std::string,double> goal_constraints_;
00216   std::map<std::string,double> trajectory_constraints_;
00217   double goal_time_constraint_;
00218   double stopped_velocity_tolerance_;
00219 
00220   control_msgs::FollowJointTrajectoryFeedbackConstPtr last_controller_state_;
00221 
00222   void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
00223   {
00224     //ROS_DEBUG("Checking controller state feedback");
00225     last_controller_state_ = msg;
00226     ros::Time now = ros::Time::now();
00227 
00228     if (!has_active_goal_)
00229     {
00230       //ROS_DEBUG("No active goal, ignoring feedback");
00231       return;
00232     }
00233     if (current_traj_.points.empty())
00234     {
00235       ROS_DEBUG("Current trajecotry is empty, ignoring feedback");
00236       return;
00237     }
00238     /* NOT CONCERNED ABOUT TRAJECTORY TIMING AT THIS POINT
00239     if (now < current_traj_.header.stamp + current_traj_.points[0].time_from_start)
00240       return;
00241     */
00242 
00243     if (!setsEqual(joint_names_, msg->joint_names))
00244     {
00245       ROS_ERROR("Joint names from the controller don't match our joint names.");
00246       return;
00247     }
00248 
00249     // Checking for goal constraints
00250     // Checks that we have ended inside the goal constraints
00251 
00252     ROS_DEBUG("Checking goal contraints");
00253     bool inside_goal_constraints = true;
00254     int last = current_traj_.points.size() - 1;
00255     for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints; ++i)
00256     {
00257       double abs_error = fabs(msg->actual.positions[i]-current_traj_.points[last].positions[i]);
00258       double goal_constraint = goal_constraints_[msg->joint_names[i]];
00259       if (goal_constraint >= 0 && abs_error > goal_constraint)
00260       {
00261         inside_goal_constraints = false;
00262       }
00263       ROS_DEBUG("Checking constraint: %f, abs_errs: %f", goal_constraint, abs_error);
00264     }
00265 
00266     if (inside_goal_constraints)
00267     {
00268       ROS_INFO("Inside goal contraints, return success for action");
00269       active_goal_.setSucceeded();
00270       has_active_goal_ = false;
00271     }
00272     // Verifies that the controller has stayed within the trajectory constraints.
00273     /*  DISABLING THIS MORE COMPLICATED GOAL CHECKING AND ERROR DETECTION
00274 
00275 
00276     int last = current_traj_.points.size() - 1;
00277     ros::Time end_time = current_traj_.header.stamp + current_traj_.points[last].time_from_start;
00278     if (now < end_time)
00279     {
00280       // Checks that the controller is inside the trajectory constraints.
00281       for (size_t i = 0; i < msg->joint_names.size(); ++i)
00282       {
00283         double abs_error = fabs(msg->error.positions[i]);
00284         double constraint = trajectory_constraints_[msg->joint_names[i]];
00285         if (constraint >= 0 && abs_error > constraint)
00286         {
00287           // Stops the controller.
00288           trajectory_msgs::JointTrajectory empty;
00289           empty.joint_names = joint_names_;
00290           pub_controller_command_.publish(empty);
00291 
00292           active_goal_.setAborted();
00293           has_active_goal_ = false;
00294           ROS_WARN("Aborting because we would up outside the trajectory constraints");
00295           return;
00296         }
00297       }
00298     }
00299     else
00300     {
00301       // Checks that we have ended inside the goal constraints
00302       bool inside_goal_constraints = true;
00303       for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints; ++i)
00304       {
00305         double abs_error = fabs(msg->error.positions[i]);
00306         double goal_constraint = goal_constraints_[msg->joint_names[i]];
00307         if (goal_constraint >= 0 && abs_error > goal_constraint)
00308           inside_goal_constraints = false;
00309 
00310         // It's important to be stopped if that's desired.
00311         if (fabs(msg->desired.velocities[i]) < 1e-6)
00312         {
00313           if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_)
00314             inside_goal_constraints = false;
00315         }
00316       }
00317 
00318       if (inside_goal_constraints)
00319       {
00320         active_goal_.setSucceeded();
00321         has_active_goal_ = false;
00322       }
00323       else if (now < end_time + ros::Duration(goal_time_constraint_))
00324       {
00325         // Still have some time left to make it.
00326       }
00327       else
00328       {
00329         ROS_WARN("Aborting because we wound up outside the goal constraints");
00330         active_goal_.setAborted();
00331         has_active_goal_ = false;
00332       }
00333 
00334     }
00335     */
00336   }
00337 };
00338 
00339 
00340 int main(int argc, char** argv)
00341 {
00342   ros::init(argc, argv, "joint_trajectory_action_node");
00343   ros::NodeHandle node;//("~");
00344   JointTrajectoryExecuter jte(node);
00345 
00346   ros::spin();
00347 
00348   return 0;
00349 }
00350 
00351 
00352 
00353 


adept_common
Author(s): root
autogenerated on Fri Jan 3 2014 11:05:39