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 #include <industrial_robot_client/joint_trajectory_action.h>
00033 #include <industrial_robot_client/utils.h>
00034 #include <industrial_utils/param_utils.h>
00035 #include <industrial_utils/utils.h>
00036 
00037 namespace industrial_robot_client
00038 {
00039 namespace joint_trajectory_action
00040 {
00041 
00042 const double JointTrajectoryAction::WATCHDOG_PERIOD_ = 1.0;
00043 const double JointTrajectoryAction::DEFAULT_GOAL_THRESHOLD_ = 0.01;
00044 
00045 JointTrajectoryAction::JointTrajectoryAction() :
00046     action_server_(node_, "joint_trajectory_action", boost::bind(&JointTrajectoryAction::goalCB, this, _1),
00047                    boost::bind(&JointTrajectoryAction::cancelCB, this, _1), false), has_active_goal_(false),
00048                        controller_alive_(false), has_moved_once_(false)
00049 {
00050   ros::NodeHandle pn("~");
00051 
00052   pn.param("constraints/goal_threshold", goal_threshold_, DEFAULT_GOAL_THRESHOLD_);
00053 
00054   if (!industrial_utils::param::getJointNames("controller_joint_names", "robot_description", joint_names_))
00055     ROS_ERROR("Failed to initialize joint_names.");
00056 
00057   // The controller joint names parameter includes empty joint names for those joints not supported
00058   // by the controller.  These are removed since the trajectory action should ignore these.
00059   std::remove(joint_names_.begin(), joint_names_.end(), std::string());
00060   ROS_INFO_STREAM("Filtered joint names to " << joint_names_.size() << " joints");
00061 
00062   pub_trajectory_command_ = node_.advertise<trajectory_msgs::JointTrajectory>("joint_path_command", 1);
00063   sub_trajectory_state_ = node_.subscribe("feedback_states", 1, &JointTrajectoryAction::controllerStateCB, this);
00064   sub_robot_status_ = node_.subscribe("robot_status", 1, &JointTrajectoryAction::robotStatusCB, this);
00065 
00066   watchdog_timer_ = node_.createTimer(ros::Duration(WATCHDOG_PERIOD_), &JointTrajectoryAction::watchdog, this, true);
00067   action_server_.start();
00068 }
00069 
00070 JointTrajectoryAction::~JointTrajectoryAction()
00071 {
00072 }
00073 
00074 void JointTrajectoryAction::robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg)
00075 {
00076   last_robot_status_ = msg; //caching robot status for later use.
00077   has_moved_once_ = has_moved_once_ ? true : (last_robot_status_->in_motion.val == industrial_msgs::TriState::TRUE);
00078 }
00079 
00080 void JointTrajectoryAction::watchdog(const ros::TimerEvent &e)
00081 {
00082   // Some debug logging
00083   if (!last_trajectory_state_)
00084   {
00085     ROS_DEBUG("Waiting for subscription to joint trajectory state");
00086   }
00087 
00088   ROS_WARN("Trajectory state not received for %f seconds", WATCHDOG_PERIOD_);
00089   controller_alive_ = false;
00090 
00091 
00092   // Aborts the active goal if the controller does not appear to be active.
00093   if (has_active_goal_)
00094   {
00095     // last_trajectory_state_ is null if the subscriber never makes a connection
00096     if (!last_trajectory_state_)
00097     {
00098       ROS_WARN("Aborting goal because we have never heard a controller state message.");
00099     }
00100     else
00101     {
00102       ROS_WARN_STREAM(
00103           "Aborting goal because we haven't heard from the controller in " << WATCHDOG_PERIOD_ << " seconds");
00104     }
00105 
00106     abortGoal();
00107   }
00108 }
00109 
00110 void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh)
00111 {
00112   ROS_INFO("Received new goal");
00113 
00114   // reject all goals as long as we haven't heard from the remote controller
00115   if (!controller_alive_)
00116   {
00117     ROS_ERROR("Joint trajectory action rejected: waiting for (initial) feedback from controller");
00118     control_msgs::FollowJointTrajectoryResult rslt;
00119     rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
00120     gh.setRejected(rslt, "Waiting for (initial) feedback from controller");
00121 
00122     // no point in continuing: already rejected
00123     return;
00124   }
00125 
00126   if (!gh.getGoal()->trajectory.points.empty())
00127   {
00128     if (industrial_utils::isSimilar(joint_names_, gh.getGoal()->trajectory.joint_names))
00129     {
00130 
00131       // Cancels the currently active goal.
00132       if (has_active_goal_)
00133       {
00134         ROS_WARN("Received new goal, canceling current goal");
00135         abortGoal();
00136       }
00137       
00138       gh.setAccepted();
00139       active_goal_ = gh;
00140       has_active_goal_ = true;
00141       time_to_check_ = ros::Time::now() + 
00142           ros::Duration(active_goal_.getGoal()->trajectory.points.back().time_from_start.toSec() / 2.0);
00143       has_moved_once_ = false;
00144 
00145       ROS_INFO("Publishing trajectory");
00146 
00147       current_traj_ = active_goal_.getGoal()->trajectory;
00148       pub_trajectory_command_.publish(current_traj_);
00149     
00150     }
00151     else
00152     {
00153       ROS_ERROR("Joint trajectory action failing on invalid joints");
00154       control_msgs::FollowJointTrajectoryResult rslt;
00155       rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
00156       gh.setRejected(rslt, "Joint names do not match");
00157     }
00158   }
00159   else
00160   {
00161     ROS_ERROR("Joint trajectory action failed on empty trajectory");
00162     control_msgs::FollowJointTrajectoryResult rslt;
00163     rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
00164     gh.setRejected(rslt, "Empty trajectory");
00165   }
00166 
00167   // Adding some informational log messages to indicate unsupported goal constraints
00168   if (gh.getGoal()->goal_time_tolerance.toSec() > 0.0)
00169   {
00170     ROS_WARN_STREAM("Ignoring goal time tolerance in action goal, may be supported in the future");
00171   }
00172   if (!gh.getGoal()->goal_tolerance.empty())
00173   {
00174     ROS_WARN_STREAM(
00175         "Ignoring goal tolerance in action, using paramater tolerance of " << goal_threshold_ << " instead");
00176   }
00177   if (!gh.getGoal()->path_tolerance.empty())
00178   {
00179     ROS_WARN_STREAM("Ignoring goal path tolerance, option not supported by ROS-Industrial drivers");
00180   }
00181 }
00182 
00183 void JointTrajectoryAction::cancelCB(JointTractoryActionServer::GoalHandle gh)
00184 {
00185   ROS_DEBUG("Received action cancel request");
00186   if (active_goal_ == gh)
00187   {
00188     // Stops the controller.
00189     trajectory_msgs::JointTrajectory empty;
00190     empty.joint_names = joint_names_;
00191     pub_trajectory_command_.publish(empty);
00192 
00193     // Marks the current goal as canceled.
00194     active_goal_.setCanceled();
00195     has_active_goal_ = false;
00196   }
00197   else
00198   {
00199     ROS_WARN("Active goal and goal cancel do not match, ignoring cancel request");
00200   }
00201 }
00202 
00203 void JointTrajectoryAction::controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
00204 {
00205   ROS_DEBUG("Checking controller state feedback");
00206   last_trajectory_state_ = msg;
00207   controller_alive_ = true;
00208 
00209   watchdog_timer_.stop();
00210   watchdog_timer_.start();
00211 
00212   if (!has_active_goal_)
00213   {
00214     //ROS_DEBUG("No active goal, ignoring feedback");
00215     return;
00216   }
00217   if (current_traj_.points.empty())
00218   {
00219     ROS_DEBUG("Current trajectory is empty, ignoring feedback");
00220     return;
00221   }
00222 
00223   if (!industrial_utils::isSimilar(joint_names_, msg->joint_names))
00224   {
00225     ROS_ERROR("Joint names from the controller don't match our joint names.");
00226     return;
00227   }
00228 
00229   if (!has_moved_once_ && (ros::Time::now() < time_to_check_))
00230   {
00231     ROS_DEBUG("Waiting to check for goal completion until halfway through trajectory");
00232     return;
00233   }
00234 
00235   // Checking for goal constraints
00236   // Checks that we have ended inside the goal constraints and has motion stopped
00237 
00238   ROS_DEBUG("Checking goal constraints");
00239   if (withinGoalConstraints(last_trajectory_state_, current_traj_))
00240   {
00241     if (last_robot_status_)
00242     {
00243       // Additional check for motion stoppage since the controller goal may still
00244       // be moving.  The current robot driver calls a motion stop if it receives
00245       // a new trajectory while it is still moving.  If the driver is not publishing
00246       // the motion state (i.e. old driver), this will still work, but it warns you.
00247       if (last_robot_status_->in_motion.val == industrial_msgs::TriState::FALSE)
00248       {
00249         ROS_INFO("Inside goal constraints, stopped moving, return success for action");
00250         active_goal_.setSucceeded();
00251         has_active_goal_ = false;
00252       }
00253       else if (last_robot_status_->in_motion.val == industrial_msgs::TriState::UNKNOWN)
00254       {
00255         ROS_INFO("Inside goal constraints, return success for action");
00256         ROS_WARN("Robot status in motion unknown, the robot driver node and controller code should be updated");
00257         active_goal_.setSucceeded();
00258         has_active_goal_ = false;
00259       }
00260       else
00261       {
00262         ROS_DEBUG("Within goal constraints but robot is still moving");
00263       }
00264     }
00265     else
00266     {
00267       ROS_INFO("Inside goal constraints, return success for action");
00268       ROS_WARN("Robot status is not being published the robot driver node and controller code should be updated");
00269       active_goal_.setSucceeded();
00270       has_active_goal_ = false;
00271     }
00272   }
00273 }
00274 
00275 void JointTrajectoryAction::abortGoal()
00276 {
00277   // Stops the controller.
00278   trajectory_msgs::JointTrajectory empty;
00279   pub_trajectory_command_.publish(empty);
00280 
00281   // Marks the current goal as aborted.
00282   active_goal_.setAborted();
00283   has_active_goal_ = false;
00284 }
00285 
00286 bool JointTrajectoryAction::withinGoalConstraints(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg,
00287                                                   const trajectory_msgs::JointTrajectory & traj)
00288 {
00289   bool rtn = false;
00290   if (traj.points.empty())
00291   {
00292     ROS_WARN("Empty joint trajectory passed to check goal constraints, return false");
00293     rtn = false;
00294   }
00295   else
00296   {
00297     int last_point = traj.points.size() - 1;
00298 
00299     if (industrial_robot_client::utils::isWithinRange(last_trajectory_state_->joint_names,
00300                                                       last_trajectory_state_->actual.positions, traj.joint_names,
00301                                                       traj.points[last_point].positions, goal_threshold_))
00302     {
00303       rtn = true;
00304     }
00305     else
00306     {
00307       rtn = false;
00308     }
00309   }
00310   return rtn;
00311 }
00312 
00313 } //joint_trajectory_action
00314 } //industrial_robot_client
00315 


industrial_robot_client
Author(s): Jeremy Zoss
autogenerated on Tue Jan 17 2017 21:10:11