fs100_joint_trajectory_streamer.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2013, 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 "fs100/fs100_joint_trajectory_streamer.h"
00033 #include "fs100/simple_message/motoman_motion_reply_message.h"
00034 #include "simple_message/messages/joint_traj_pt_full_message.h"
00035 #include "industrial_robot_client/utils.h"
00036 #include "industrial_utils/param_utils.h"
00037 
00038 using namespace industrial::simple_message;
00039 using industrial::joint_data::JointData;
00040 using industrial::joint_traj_pt_full::JointTrajPtFull;
00041 using industrial::joint_traj_pt_full_message::JointTrajPtFullMessage;
00042 using motoman::simple_message::motion_reply_message::MotionReplyMessage;
00043 namespace TransferStates = industrial_robot_client::joint_trajectory_streamer::TransferStates;
00044 namespace MotionReplyResults = motoman::simple_message::motion_reply::MotionReplyResults;
00045 
00046 namespace motoman
00047 {
00048 namespace fs100_joint_trajectory_streamer
00049 {
00050 
00051 #define ROS_ERROR_RETURN(rtn,...) do {ROS_ERROR(__VA_ARGS__); return(rtn);} while(0)
00052 
00053 // override init() to read "robot_id" parameter and subscribe to joint_states
00054 bool FS100_JointTrajectoryStreamer::init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
00055                                    const std::map<std::string, double> &velocity_limits)
00056 {
00057   bool rtn = true;
00058 
00059   ROS_INFO("FS100_JointTrajectoryStreamer: init");
00060 
00061   rtn &= JointTrajectoryStreamer::init(connection, joint_names, velocity_limits);
00062 
00063   // try to read robot_id parameter, if none specified
00064   if ( (robot_id_ < 0) )
00065     node_.param("robot_id", robot_id_, 0);
00066 
00067   rtn &= motion_ctrl_.init(connection, robot_id_);
00068 
00069   return rtn;
00070 }
00071 
00072 FS100_JointTrajectoryStreamer::~FS100_JointTrajectoryStreamer()
00073 {
00074   //TODO Find better place to call StopTrajMode
00075   motion_ctrl_.setTrajMode(false);   // release TrajMode, so INFORM jobs can run
00076 }
00077 
00078 // override create_message to generate JointTrajPtFull message (instead of default JointTrajPt)
00079 bool FS100_JointTrajectoryStreamer::create_message(int seq, const trajectory_msgs::JointTrajectoryPoint &pt, SimpleMessage *msg)
00080 {
00081   JointTrajPtFull msg_data;
00082   JointData values;
00083 
00084   // copy position data
00085   if (!pt.positions.empty())
00086   {
00087     if (VectorToJointData(pt.positions, values))
00088       msg_data.setPositions(values);
00089     else
00090       ROS_ERROR_RETURN(false, "Failed to copy position data to JointTrajPtFullMessage");
00091   }
00092   else
00093     msg_data.clearPositions();
00094 
00095   // copy velocity data
00096   if (!pt.velocities.empty())
00097   {
00098     if (VectorToJointData(pt.velocities, values))
00099       msg_data.setVelocities(values);
00100     else
00101       ROS_ERROR_RETURN(false, "Failed to copy velocity data to JointTrajPtFullMessage");
00102   }
00103   else
00104     msg_data.clearVelocities();
00105 
00106   // copy acceleration data
00107   if (!pt.accelerations.empty())
00108   {
00109     if (VectorToJointData(pt.accelerations, values))
00110       msg_data.setAccelerations(values);
00111     else
00112       ROS_ERROR_RETURN(false, "Failed to copy acceleration data to JointTrajPtFullMessage");
00113   }
00114   else
00115     msg_data.clearAccelerations();
00116 
00117   // copy scalar data
00118   msg_data.setRobotID(robot_id_);
00119   msg_data.setSequence(seq);
00120   msg_data.setTime(pt.time_from_start.toSec());
00121 
00122   // convert to message
00123   JointTrajPtFullMessage jtpf_msg;
00124   jtpf_msg.init(msg_data);
00125 
00126   return jtpf_msg.toRequest(*msg);  // assume "request" COMM_TYPE for now
00127 }
00128 
00129 bool FS100_JointTrajectoryStreamer::VectorToJointData(const std::vector<double> &vec,
00130                                                       JointData &joints)
00131 {
00132   if ( vec.size() > joints.getMaxNumJoints() )
00133     ROS_ERROR_RETURN(false, "Failed to copy to JointData.  Len (%d) out of range (0 to %d)",
00134                      vec.size(), joints.getMaxNumJoints());
00135 
00136   joints.init();
00137   for (int i=0; i<vec.size(); ++i)
00138     joints.setJoint(i, vec[i]);
00139 
00140   return true;
00141 }
00142 
00143 // override send_to_robot to provide controllerReady() and setTrajMode() calls
00144 bool FS100_JointTrajectoryStreamer::send_to_robot(const std::vector<SimpleMessage>& messages)
00145 {
00146   if (!motion_ctrl_.controllerReady() && !motion_ctrl_.setTrajMode(true))
00147     ROS_ERROR_RETURN(false, "Failed to initialize MotoRos motion.  Trajectory ABORTED.  Correct issue and re-send trajectory.");
00148 
00149   return JointTrajectoryStreamer::send_to_robot(messages);
00150 }
00151 
00152 // override streamingThread, to provide check/retry of MotionReply.result=BUSY
00153 void FS100_JointTrajectoryStreamer::streamingThread()
00154 {
00155   int connectRetryCount = 1;
00156 
00157   ROS_INFO("Starting FS100 joint trajectory streamer thread");
00158   while (ros::ok())
00159   {
00160     ros::Duration(0.005).sleep();
00161 
00162     // automatically re-establish connection, if required
00163     if (connectRetryCount-- > 0)
00164     {
00165       ROS_INFO("Connecting to robot motion server");
00166       this->connection_->makeConnect();
00167       ros::Duration(0.250).sleep();  // wait for connection
00168 
00169       if (this->connection_->isConnected())
00170         connectRetryCount = 0;
00171       else if (connectRetryCount <= 0)
00172       {
00173         ROS_ERROR("Timeout connecting to robot controller.  Send new motion command to retry.");
00174         this->state_ = TransferStates::IDLE;
00175       }
00176       continue;
00177     }
00178 
00179     this->mutex_.lock();
00180 
00181     SimpleMessage msg, tmpMsg, reply;
00182 
00183     switch (this->state_)
00184     {
00185       case TransferStates::IDLE:
00186         ros::Duration(0.250).sleep();  //  slower loop while waiting for new trajectory
00187         break;
00188 
00189       case TransferStates::STREAMING:
00190         if (this->current_point_ >= (int)this->current_traj_.size())
00191         {
00192           ROS_INFO("Trajectory streaming complete, setting state to IDLE");
00193           this->state_ = TransferStates::IDLE;
00194           break;
00195         }
00196 
00197         if (!this->connection_->isConnected())
00198         {
00199           ROS_DEBUG("Robot disconnected.  Attempting reconnect...");
00200           connectRetryCount = 5;
00201           break;
00202         }
00203 
00204         tmpMsg = this->current_traj_[this->current_point_];
00205         msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST,
00206                  ReplyTypes::INVALID, tmpMsg.getData());  // set commType=REQUEST
00207 
00208         ROS_DEBUG("Sending joint trajectory point");
00209         if (!this->connection_->sendAndReceiveMsg(msg, reply, false))
00210           ROS_WARN("Failed sent joint point, will try again");
00211         else
00212         {
00213           MotionReplyMessage reply_status;
00214           if (!reply_status.init(reply))
00215           {
00216             ROS_ERROR("Aborting trajectory: Unable to parse JointTrajectoryPoint reply");
00217             this->state_ = TransferStates::IDLE;
00218             break;
00219           }
00220 
00221           if (reply_status.reply_.getResult() == MotionReplyResults::SUCCESS)
00222           {
00223             ROS_DEBUG("Point[%d of %d] sent to controller",
00224                      this->current_point_, (int)this->current_traj_.size());
00225             this->current_point_++;
00226           }
00227           else if (reply_status.reply_.getResult() == MotionReplyResults::BUSY)
00228             break;  // silently retry sending this point
00229           else
00230           {
00231             ROS_ERROR_STREAM("Aborting Trajectory.  Failed to send point"
00232                              << " (#" << this->current_point_ << "): "
00233                              << FS100_MotionCtrl::getErrorString(reply_status.reply_));
00234             this->state_ = TransferStates::IDLE;
00235             break;
00236           }
00237         }
00238 
00239         break;
00240       default:
00241         ROS_ERROR("Joint trajectory streamer: unknown state");
00242         this->state_ = TransferStates::IDLE;
00243         break;
00244     }
00245 
00246     this->mutex_.unlock();
00247   }
00248 
00249   ROS_WARN("Exiting trajectory streamer thread");
00250 }
00251 
00252 // override trajectoryStop to send MotionCtrl message
00253 void FS100_JointTrajectoryStreamer::trajectoryStop()
00254 {
00255   this->state_ = TransferStates::IDLE;  // stop sending trajectory points
00256   motion_ctrl_.stopTrajectory();
00257 }
00258 
00259 // override is_valid to include FS100-specific checks
00260 bool FS100_JointTrajectoryStreamer::is_valid(const trajectory_msgs::JointTrajectory &traj)
00261 {
00262   if (!JointTrajectoryInterface::is_valid(traj))
00263     return false;
00264 
00265   for (int i=0; i<traj.points.size(); ++i)
00266   {
00267     const trajectory_msgs::JointTrajectoryPoint &pt = traj.points[i];
00268 
00269     // FS100 requires valid velocity data
00270     if (pt.velocities.empty())
00271       ROS_ERROR_RETURN(false, "Validation failed: Missing velocity data for trajectory pt %d", i);
00272   }
00273 
00274   if ((cur_joint_pos_.header.stamp - ros::Time::now()).toSec() > pos_stale_time_)
00275     ROS_ERROR_RETURN(false, "Validation failed: Can't get current robot position.");
00276 
00277   // FS100 requires trajectory start at current position
00278   namespace IRC_utils = industrial_robot_client::utils;
00279   if (!IRC_utils::isWithinRange(cur_joint_pos_.name, cur_joint_pos_.position,
00280                                 traj.joint_names, traj.points[0].positions,
00281                                 start_pos_tol_))
00282   {
00283     ROS_ERROR_RETURN(false, "Validation failed: Trajectory doesn't start at current position.");
00284   }
00285 
00286   return true;
00287 }
00288 
00289 
00290 } //fs100_joint_trajectory_streamer
00291 } //motoman
00292 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


fs100
Author(s): Jeremy Zoss (Southwest Research Institute)
autogenerated on Mon May 27 2013 19:43:14