fs100_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 #include "fs100/fs100_motion_ctrl.h"
00033 #include "fs100/simple_message/motoman_motion_ctrl_message.h"
00034 #include "fs100/simple_message/motoman_motion_reply_message.h"
00035 #include "ros/ros.h"
00036 #include "simple_message/simple_message.h"
00037 
00038 using namespace motoman::simple_message::motion_ctrl;
00039 using namespace motoman::simple_message::motion_reply;
00040 using motoman::simple_message::motion_ctrl_message::MotionCtrlMessage;
00041 using motoman::simple_message::motion_reply_message::MotionReplyMessage;
00042 using industrial::simple_message::SimpleMessage;
00043 
00044 namespace motoman
00045 {
00046 namespace fs100_motion_ctrl
00047 {
00048 
00049 bool FS100_MotionCtrl::init(SmplMsgConnection* connection, int robot_id)
00050 {
00051   connection_ = connection;
00052   robot_id_ = robot_id;
00053   return true;
00054 }
00055 
00056 bool FS100_MotionCtrl::controllerReady()
00057 {
00058   std::string err_str;
00059   MotionReply reply;
00060 
00061   if (!sendAndReceive(MotionControlCmds::CHECK_MOTION_READY, reply))
00062   {
00063     ROS_ERROR("Failed to send CHECK_MOTION_READY command");
00064     return false;
00065   }
00066 
00067  return (reply.getResult() == MotionReplyResults::TRUE);
00068 }
00069 
00070 
00071 bool FS100_MotionCtrl::setTrajMode(bool enable)
00072 {
00073   MotionReply reply;
00074   MotionControlCmd cmd = enable ? MotionControlCmds::START_TRAJ_MODE : MotionControlCmds::STOP_TRAJ_MODE;
00075 
00076   if (!sendAndReceive(cmd, reply))
00077   {
00078     ROS_ERROR("Failed to send TRAJ_MODE command");
00079     return false;
00080   }
00081 
00082   if (reply.getResult() != MotionReplyResults::SUCCESS)
00083   {
00084     ROS_ERROR_STREAM("Failed to set TrajectoryMode: " << getErrorString(reply));
00085     return false;
00086   }
00087 
00088   return true;
00089 }
00090 
00091 bool FS100_MotionCtrl::stopTrajectory()
00092 {
00093   MotionReply reply;
00094 
00095   if (!sendAndReceive(MotionControlCmds::STOP_MOTION, reply))
00096   {
00097     ROS_ERROR("Failed to send STOP_MOTION command");
00098     return false;
00099   }
00100 
00101   if (reply.getResult() != MotionReplyResults::SUCCESS)
00102   {
00103     ROS_ERROR_STREAM("Failed to Stop Motion: " << getErrorString(reply));
00104     return false;
00105   }
00106 
00107   return true;
00108 }
00109 
00110 bool FS100_MotionCtrl::sendAndReceive(MotionControlCmd command, MotionReply &reply)
00111 {
00112   SimpleMessage req, res;
00113   MotionCtrl data;
00114   MotionCtrlMessage ctrl_msg;
00115   MotionReplyMessage ctrl_reply;
00116 
00117   data.init(robot_id_, 0, command, 0);
00118   ctrl_msg.init(data);
00119   ctrl_msg.toRequest(req);
00120 
00121   if (!this->connection_->sendAndReceiveMsg(req, res))
00122   {
00123     ROS_ERROR("Failed to send MotionCtrl message");
00124     return false;
00125   }
00126 
00127   ctrl_reply.init(res);
00128   reply.copyFrom(ctrl_reply.reply_);
00129 
00130   return true;
00131 }
00132 
00133 std::string FS100_MotionCtrl::getErrorString(const MotionReply &reply)
00134 {
00135   std::ostringstream ss;
00136   ss << reply.getResultString() << " (" << reply.getResult() << ")";
00137   ss << " : ";
00138   ss << reply.getSubcodeString() << " (" << reply.getSubcode() << ")";
00139   return ss.str();
00140 }
00141 
00142 
00143 } //fs100_motion_ctrl
00144 } //motoman
00145 
 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