motoman_motion_ctrl.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 #ifdef ROS
00033 #include "motoman_driver/simple_message/motoman_motion_ctrl.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 "shared_types.h"
00041 #include "log_wrapper.h"
00042 #endif
00043 
00044 using industrial::shared_types::shared_int;
00045 using industrial::shared_types::shared_real;
00046 
00047 namespace motoman
00048 {
00049 namespace simple_message
00050 {
00051 namespace motion_ctrl
00052 {
00053 MotionCtrl::MotionCtrl(void)
00054 {
00055   this->init();
00056 }
00057 MotionCtrl::~MotionCtrl(void)
00058 {
00059 }
00060 
00061 void MotionCtrl::init()
00062 {
00063   this->init(0, 0, MotionControlCmds::UNDEFINED, 0);
00064 }
00065 
00066 void MotionCtrl::init(shared_int robot_id, shared_int sequence,
00067                       MotionControlCmd command, shared_real data)
00068 {
00069   this->setRobotID(robot_id);
00070   this->setSequence(sequence);
00071   this->setCommand(command);
00072   this->clearData();
00073   this->setData(0, data);
00074 }
00075 
00076 void MotionCtrl::copyFrom(MotionCtrl &src)
00077 {
00078   this->setRobotID(src.getRobotID());
00079   this->setSequence(src.getSequence());
00080   this->setCommand(src.getCommand());
00081   for (size_t i = 0; i < MAX_DATA_CNT; ++i)
00082     this->setData(i, src.getData(i));
00083 }
00084 
00085 bool MotionCtrl::operator==(MotionCtrl &rhs)
00086 {
00087   bool rslt = this->robot_id_ == rhs.robot_id_ &&
00088               this->sequence_ == rhs.sequence_ &&
00089               this->command_ == rhs.command_;
00090 
00091   for (size_t i = 0; i < MAX_DATA_CNT; ++i)
00092     rslt &= (this->data_[i] == rhs.data_[i]);
00093 
00094   return rslt;
00095 }
00096 
00097 bool MotionCtrl::load(industrial::byte_array::ByteArray *buffer)
00098 {
00099   LOG_COMM("Executing MotionCtrl command load");
00100 
00101   if (!buffer->load(this->robot_id_))
00102   {
00103     LOG_ERROR("Failed to load MotionCtrl robot_id");
00104     return false;
00105   }
00106 
00107   if (!buffer->load(this->sequence_))
00108   {
00109     LOG_ERROR("Failed to load MotionCtrl sequence");
00110     return false;
00111   }
00112 
00113   if (!buffer->load(this->command_))
00114   {
00115     LOG_ERROR("Failed to load MotionCtrl command");
00116     return false;
00117   }
00118 
00119   for (size_t i = 0; i < MAX_DATA_CNT; ++i)
00120   {
00121     shared_real value = this->getData(i);
00122     if (!buffer->load(value))
00123     {
00124       LOG_ERROR("Failed to load MotionCtrl data element %d from data[%d]", static_cast<int>(i), buffer->getBufferSize());
00125       return false;
00126     }
00127   }
00128 
00129   LOG_COMM("MotionCtrl data successfully loaded");
00130   return true;
00131 }
00132 
00133 bool MotionCtrl::unload(industrial::byte_array::ByteArray *buffer)
00134 {
00135   LOG_COMM("Executing MotionCtrl command unload");
00136 
00137   for (int i = MAX_DATA_CNT - 1; i >= 0; --i)
00138   {
00139     shared_real value;
00140     if (!buffer->unload(value))
00141     {
00142       LOG_ERROR("Failed to unload message data element: %d from data[%d]", static_cast<int>(i), buffer->getBufferSize());
00143       return false;
00144     }
00145     this->setData(i, value);
00146   }
00147 
00148   if (!buffer->unload(this->command_))
00149   {
00150     LOG_ERROR("Failed to unload MotionCtrl command");
00151     return false;
00152   }
00153 
00154   if (!buffer->unload(this->sequence_))
00155   {
00156     LOG_ERROR("Failed to unload MotionCtrl sequence");
00157     return false;
00158   }
00159 
00160   if (!buffer->unload(this->robot_id_))
00161   {
00162     LOG_ERROR("Failed to unload MotionCtrl robot_id");
00163     return false;
00164   }
00165 
00166   LOG_COMM("MotionCtrl data successfully unloaded");
00167   return true;
00168 }
00169 
00170 }  // namespace motion_ctrl
00171 }  // namespace simple_message
00172 }  // namespace motoman


motoman_driver
Author(s): Jeremy Zoss (Southwest Research Institute)
autogenerated on Sat Jun 8 2019 19:06:57