motoman_motion_reply.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 #ifdef ROS
00032 #include "fs100/simple_message/motoman_motion_ctrl.h"
00033 #include "fs100/simple_message/motoman_motion_reply.h"
00034 #include "simple_message/shared_types.h"
00035 #include "simple_message/log_wrapper.h"
00036 #endif
00037 
00038 #ifdef MOTOPLUS
00039 #include "motoman_motion_ctrl.h"
00040 #include "motoman_motion_reply.h"
00041 #include "shared_types.h"
00042 #include "log_wrapper.h"
00043 #endif
00044 
00045 using namespace industrial::shared_types;
00046 using namespace motoman::simple_message::motion_ctrl;
00047 
00048 namespace motoman
00049 {
00050 namespace simple_message
00051 {
00052 namespace motion_reply
00053 {
00054 
00055 MotionReply::MotionReply(void)
00056 {
00057   this->init();
00058 }
00059 MotionReply::~MotionReply(void)
00060 {
00061 
00062 }
00063 
00064 void MotionReply::init()
00065 {
00066   this->init(0, 0, MotionControlCmds::UNDEFINED, MotionReplyResults::SUCCESS, 0, 0);
00067 }
00068 
00069 void MotionReply::init(shared_int robot_id, shared_int sequence,
00070                       shared_int command, MotionReplyResult result,
00071                       shared_int subcode, shared_real data)
00072 {
00073   this->setRobotID(robot_id);
00074   this->setSequence(sequence);
00075   this->setCommand(command);
00076   this->setResult(result);
00077   this->setSubcode(subcode);
00078   this->clearData();
00079   this->setData(0,data);
00080 }
00081 
00082 std::string MotionReply::getResultString(shared_int code)
00083 {
00084   switch (code)
00085   {
00086     case MotionReplyResults::SUCCESS:
00087       return "Success";
00088     case MotionReplyResults::BUSY:
00089       return "Busy";
00090     case MotionReplyResults::FAILURE:
00091       return "Failed";
00092     case MotionReplyResults::INVALID:
00093       return "Invalid message";
00094     case MotionReplyResults::ALARM:
00095       return "Controller alarm";
00096     case MotionReplyResults::NOT_READY:
00097       return "Not Ready";
00098     case MotionReplyResults::MP_FAILURE:
00099       return "MotoPlus Error";
00100     default:
00101       return "Unknown";
00102   }
00103 }
00104 
00105 std::string MotionReply::getSubcodeString(shared_int code)
00106 {
00107   switch (code)
00108   {
00109     case MotionReplySubcodes::Invalid::UNSPECIFIED:
00110       return "Unknown";
00111     case MotionReplySubcodes::Invalid::MSGSIZE:
00112       return "Invalid message size";
00113     case MotionReplySubcodes::Invalid::MSGHEADER:
00114       return "Invalid header";
00115     case MotionReplySubcodes::Invalid::MSGTYPE:
00116       return "Invalid message type";
00117     case MotionReplySubcodes::Invalid::GROUPNO:
00118       return "Invalid robot ID";
00119     case MotionReplySubcodes::Invalid::SEQUENCE:
00120       return "Invalid sequence ID";
00121     case MotionReplySubcodes::Invalid::COMMAND:
00122       return "Invalid command";
00123     case MotionReplySubcodes::Invalid::DATA:
00124       return "Invalid data";
00125     case MotionReplySubcodes::Invalid::DATA_START_POS:
00126       return "Trajectory start position doesn't match current robot position";
00127     case MotionReplySubcodes::Invalid::DATA_POSITION:
00128       return "Invalid position data";
00129     case MotionReplySubcodes::Invalid::DATA_SPEED:
00130       return "Invalid velocity data";
00131     case MotionReplySubcodes::Invalid::DATA_ACCEL:
00132       return "Invalid acceleration data";    
00133     case MotionReplySubcodes::Invalid::DATA_INSUFFICIENT:
00134       return "Insufficient trajectory data.  Must supply valid time, pos, and velocity fields.";
00135 
00136     case MotionReplySubcodes::NotReady::UNSPECIFIED:
00137       return "Unknown";
00138     case MotionReplySubcodes::NotReady::ALARM:
00139       return "Controller alarm active";
00140     case MotionReplySubcodes::NotReady::ERROR:
00141       return "Controller error";
00142     case MotionReplySubcodes::NotReady::ESTOP:
00143       return "E-Stop active";
00144     case MotionReplySubcodes::NotReady::NOT_PLAY:
00145       return "Controller in TEACH mode";
00146     case MotionReplySubcodes::NotReady::NOT_REMOTE:
00147       return "Controller not in REMOTE mode";
00148     case MotionReplySubcodes::NotReady::SERVO_OFF:
00149       return "Unable to enable drive power";
00150     case MotionReplySubcodes::NotReady::HOLD:
00151       return "Controller in HOLD state";
00152     case MotionReplySubcodes::NotReady::NOT_STARTED:
00153       return "MotoRos not started";
00154     case MotionReplySubcodes::NotReady::WAITING_ROS:
00155       return "Waiting on ROS";
00156 
00157     default:
00158       return "Unknown";
00159   }
00160 }
00161 
00162 void MotionReply::copyFrom(MotionReply &src)
00163 {
00164   this->setRobotID(src.getRobotID());
00165   this->setSequence(src.getSequence());
00166   this->setCommand(src.getCommand());
00167   this->setResult(src.getResult());
00168   this->setSubcode(src.getSubcode());
00169   for (size_t i=0; i<MAX_DATA_CNT; ++i)
00170     this->setData(i, src.getData(i));
00171 }
00172 
00173 bool MotionReply::operator==(MotionReply &rhs)
00174 {
00175   bool rslt = this->robot_id_ == rhs.robot_id_ &&
00176               this->sequence_ == rhs.sequence_ &&
00177               this->command_ == rhs.command_ &&
00178               this->result_ == rhs.result_ &&
00179               this->subcode_ == rhs.subcode_;
00180 
00181   for (size_t i=0; i<MAX_DATA_CNT; ++i)
00182     rslt &= (this->data_[i] == rhs.data_[i]);
00183 
00184   return rslt;
00185 }
00186 
00187 bool MotionReply::load(industrial::byte_array::ByteArray *buffer)
00188 {
00189   LOG_COMM("Executing MotionReply command load");
00190 
00191   if (!buffer->load(this->robot_id_))
00192   {
00193     LOG_ERROR("Failed to load MotionReply robot_id");
00194     return false;
00195   }
00196 
00197   if (!buffer->load(this->sequence_))
00198   {
00199     LOG_ERROR("Failed to load MotionReply sequence");
00200     return false;
00201   }
00202 
00203   if (!buffer->load(this->command_))
00204   {
00205     LOG_ERROR("Failed to load MotionReply command");
00206     return false;
00207   }
00208 
00209   if (!buffer->load(this->result_))
00210   {
00211     LOG_ERROR("Failed to load MotionReply result");
00212     return false;
00213   }
00214 
00215   if (!buffer->load(this->subcode_))
00216   {
00217     LOG_ERROR("Failed to load MotionReply subcode");
00218     return false;
00219   }
00220 
00221   for (size_t i=0; i<MAX_DATA_CNT; ++i)
00222   {
00223     shared_real value = this->getData(i);
00224     if (!buffer->load(value))
00225     {
00226       LOG_ERROR("Failed to load MotionReply data element %d from data[%d]", (int)i, buffer->getBufferSize());
00227       return false;
00228     }
00229   }
00230 
00231   LOG_COMM("MotionReply data successfully loaded");
00232   return true;
00233 }
00234 
00235 bool MotionReply::unload(industrial::byte_array::ByteArray *buffer)
00236 {
00237   LOG_COMM("Executing MotionReply command unload");
00238 
00239   for (int i=MAX_DATA_CNT-1; i>=0; --i)
00240   {
00241     shared_real value;
00242     if (!buffer->unload(value))
00243     {
00244       LOG_ERROR("Failed to unload message data element: %d from data[%d]", (int)i, buffer->getBufferSize());
00245       return false;
00246     }
00247     this->setData(i, value);
00248   }
00249 
00250   if (!buffer->unload(this->subcode_))
00251   {
00252     LOG_ERROR("Failed to unload MotionReply subcode");
00253     return false;
00254   }
00255 
00256   if (!buffer->unload(this->result_))
00257   {
00258     LOG_ERROR("Failed to unload MotionReply result");
00259     return false;
00260   }
00261 
00262   if (!buffer->unload(this->command_))
00263   {
00264     LOG_ERROR("Failed to unload MotionReply command");
00265     return false;
00266   }
00267 
00268   if (!buffer->unload(this->sequence_))
00269   {
00270     LOG_ERROR("Failed to unload MotionReply sequence");
00271     return false;
00272   }
00273 
00274   if (!buffer->unload(this->robot_id_))
00275   {
00276     LOG_ERROR("Failed to unload MotionReply robot_id");
00277     return false;
00278   }
00279 
00280   LOG_COMM("MotionReply data successfully unloaded");
00281   return true;
00282 }
00283 
00284 }
00285 }
00286 }
 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