joint_feedback_relay_handler.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/industrial_robot_client/joint_feedback_relay_handler.h"
00033 #include "simple_message/log_wrapper.h"
00034 
00035 using industrial::joint_data::JointData;
00036 using industrial::shared_types::shared_real;
00037 using namespace industrial::simple_message;
00038 
00039 namespace industrial_robot_client
00040 {
00041 namespace joint_feedback_relay_handler
00042 {
00043 
00044 bool JointFeedbackRelayHandler::init(SmplMsgConnection* connection,
00045                                      std::vector<std::string> &joint_names)
00046 {
00047   bool rtn = JointRelayHandler::init(connection, (int)StandardMsgTypes::JOINT_FEEDBACK, joint_names);
00048 
00049   // try to read robot_id parameter, if none specified
00050   if ( (robot_id_ < 0) )
00051     node_.param("robot_id", robot_id_, 0);
00052 
00053   return rtn;
00054 }
00055 
00056 
00057 bool JointFeedbackRelayHandler::create_messages(SimpleMessage& msg_in,
00058                                                 control_msgs::FollowJointTrajectoryFeedback* control_state,
00059                                                 sensor_msgs::JointState* sensor_state)
00060 {
00061   // inspect robot_id field first, to avoid "Failed to Convert" message
00062   JointFeedbackMessage tmp_msg;
00063   if (tmp_msg.init(msg_in) && (tmp_msg.getRobotID() != robot_id_))
00064   {
00065     LOG_COMM("Ignoring Message: robotID (%d) doesn't match expected (%d)",
00066              tmp_msg.getRobotID(), robot_id_);
00067     return false;
00068   }
00069 
00070   return JointRelayHandler::create_messages(msg_in, control_state, sensor_state);
00071 }
00072 
00073 bool JointFeedbackRelayHandler::convert_message(SimpleMessage& msg_in, JointTrajectoryPoint* joint_state)
00074 {
00075   JointFeedbackMessage joint_feedback_msg;
00076 
00077   if (!joint_feedback_msg.init(msg_in))
00078   {
00079     LOG_ERROR("Failed to initialize joint feedback message");
00080     return false;
00081   }
00082 
00083   return convert_message(joint_feedback_msg, joint_state);
00084 }
00085 
00086 bool JointFeedbackRelayHandler::JointDataToVector(const JointData &joints,
00087                                                   std::vector<double> &vec,
00088                                                   int len)
00089 {
00090   if ( (len<0) || (len>joints.getMaxNumJoints()) )
00091   {
00092     LOG_ERROR("Failed to copy JointData.  Len (%d) out of range (0 to %d)",
00093               len, joints.getMaxNumJoints());
00094     return false;
00095   }
00096 
00097   vec.resize(len);
00098   for (int i=0; i<len; ++i)
00099     vec[i] = joints.getJoint(i);
00100 
00101   return true;
00102 }
00103 
00104 bool JointFeedbackRelayHandler::convert_message(JointFeedbackMessage& msg_in, JointTrajectoryPoint* joint_state)
00105 {
00106   JointData values;
00107   int num_jnts = all_joint_names_.size();
00108 
00109   // copy position data
00110   if (msg_in.getPositions(values))
00111   {
00112     if (!JointDataToVector(values, joint_state->positions, num_jnts))
00113     {
00114       LOG_ERROR("Failed to parse position data from JointFeedbackMessage");
00115       return false;
00116     }
00117   } else
00118     joint_state->positions.clear();
00119 
00120   // copy velocity data
00121   if (msg_in.getVelocities(values))
00122   {
00123     if (!JointDataToVector(values, joint_state->velocities, num_jnts))
00124     {
00125       LOG_ERROR("Failed to parse velocity data from JointFeedbackMessage");
00126       return false;
00127     }
00128   } else
00129     joint_state->velocities.clear();
00130 
00131   // copy acceleration data
00132   if (msg_in.getAccelerations(values))
00133   {
00134     if (!JointDataToVector(values, joint_state->accelerations, num_jnts))
00135     {
00136       LOG_ERROR("Failed to parse acceleration data from JointFeedbackMessage");
00137       return false;
00138     }
00139   } else
00140     joint_state->accelerations.clear();
00141 
00142   // copy timestamp data
00143   shared_real value;
00144   if (msg_in.getTime(value))
00145     joint_state->time_from_start = ros::Duration(value);
00146   else
00147     joint_state->time_from_start = ros::Duration(0);
00148 
00149   return true;
00150 }
00151 
00152 }//namespace joint_feedback_relay_handler
00153 }//namespace industrial_robot_client
00154 
00155 
00156 
00157 
 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