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


joint_trajectory_action
Author(s): Stuart Glaser
autogenerated on Thu Aug 27 2015 14:23:00