joint_relay_handler.cpp
Go to the documentation of this file.
00001 /*
00002 * Software License Agreement (BSD License) 
00003 *
00004 * Copyright (c) 2011, 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 <algorithm>
00033 
00034 #include "fs100/industrial_robot_client/joint_relay_handler.h"
00035 #include "simple_message/log_wrapper.h"
00036 
00037 using industrial::message_handler::MessageHandler;
00038 using industrial::shared_types::shared_real;
00039 using industrial::smpl_msg_connection::SmplMsgConnection;
00040 using namespace industrial::simple_message;
00041 
00042 namespace industrial_robot_client
00043 {
00044 namespace joint_relay_handler
00045 {
00046 
00047 bool JointRelayHandler::init(SmplMsgConnection* connection, int msg_type, std::vector<std::string>& joint_names)
00048 {
00049   this->pub_joint_control_state_ =
00050           this->node_.advertise<control_msgs::FollowJointTrajectoryFeedback>("feedback_states", 1);
00051 
00052   this->pub_joint_sensor_state_ = this->node_.advertise<sensor_msgs::JointState>("joint_states",1);
00053 
00054   // save "complete" joint-name list, preserving any blank entries for later use
00055   this->all_joint_names_ = joint_names;
00056 
00057   return MessageHandler::init(msg_type, connection);
00058 }
00059 
00060 bool JointRelayHandler::internalCB(SimpleMessage& msg_in)
00061 {
00062   control_msgs::FollowJointTrajectoryFeedback control_state;
00063   sensor_msgs::JointState sensor_state;
00064   bool rtn = true;
00065 
00066   if (create_messages(msg_in, &control_state, &sensor_state))
00067   {
00068     this->pub_joint_control_state_.publish(control_state);
00069     this->pub_joint_sensor_state_.publish(sensor_state);
00070   }
00071   else
00072     rtn = false;
00073 
00074   // Reply back to the controller if the sender requested it.
00075   if (CommTypes::SERVICE_REQUEST == msg_in.getMessageType())
00076   {
00077     SimpleMessage reply;
00078     reply.init(msg_in.getMessageType(),
00079                CommTypes::SERVICE_REPLY,
00080                rtn ? ReplyTypes::SUCCESS : ReplyTypes::FAILURE);
00081     this->getConnection()->sendMsg(reply);
00082   }
00083 
00084   return rtn;
00085 }
00086 
00087 // TODO: Add support for other message fields (effort, desired pos)
00088 bool JointRelayHandler::create_messages(SimpleMessage& msg_in,
00089                                         control_msgs::FollowJointTrajectoryFeedback* control_state,
00090                                         sensor_msgs::JointState* sensor_state)
00091 {
00092   // read state from robot message
00093   JointTrajectoryPoint all_joint_state;
00094   if (!convert_message(msg_in, &all_joint_state))
00095   {
00096     LOG_ERROR("Failed to convert SimpleMessage");
00097     return false;
00098   }
00099 
00100   // apply transform, if required
00101   JointTrajectoryPoint xform_joint_state;
00102   if (!transform(all_joint_state, &xform_joint_state))
00103   {
00104     LOG_ERROR("Failed to transform joint state");
00105     return false;
00106   }
00107 
00108   // select specific joints for publishing
00109   JointTrajectoryPoint pub_joint_state;
00110   std::vector<std::string> pub_joint_names;
00111   if (!select(xform_joint_state, all_joint_names_, &pub_joint_state, &pub_joint_names))
00112   {
00113     LOG_ERROR("Failed to select joints for publishing");
00114     return false;
00115   }
00116 
00117   // assign values to messages
00118   *control_state = control_msgs::FollowJointTrajectoryFeedback();  // always start with a "clean" message
00119   control_state->header.stamp = ros::Time::now();
00120   control_state->joint_names = pub_joint_names;
00121   control_state->actual.positions = pub_joint_state.positions;
00122   control_state->actual.velocities = pub_joint_state.velocities;
00123   control_state->actual.accelerations = pub_joint_state.accelerations;
00124   control_state->actual.time_from_start = pub_joint_state.time_from_start;
00125 
00126   *sensor_state = sensor_msgs::JointState();  // always start with a "clean" message
00127   sensor_state->header.stamp = ros::Time::now();
00128   sensor_state->name = pub_joint_names;
00129   sensor_state->position = pub_joint_state.positions;
00130   sensor_state->velocity = pub_joint_state.velocities;
00131 
00132   return true;
00133 }
00134 
00135 bool JointRelayHandler::convert_message(SimpleMessage& msg_in, JointTrajectoryPoint* joint_state)
00136 {
00137   JointMessage joint_msg;
00138 
00139   if (!joint_msg.init(msg_in))
00140   {
00141     LOG_ERROR("Failed to initialize joint message");
00142     return false;
00143   }
00144 
00145   return convert_message(joint_msg, joint_state);
00146 }
00147 
00148 bool JointRelayHandler::convert_message(JointMessage& msg_in, JointTrajectoryPoint* joint_state)
00149 {
00150   // copy position data
00151   int num_jnts = all_joint_names_.size();
00152   joint_state->positions.resize(num_jnts);
00153   for (int i=0; i<num_jnts; ++i)
00154   {
00155     shared_real value;
00156     if (msg_in.getJoints().getJoint(i, value))
00157       joint_state->positions[i] = value;
00158     else
00159       LOG_ERROR("Failed to parse position #%d from JointMessage", i);
00160   }
00161 
00162   // these fields are not provided by JointMessage
00163   joint_state->velocities.clear();
00164   joint_state->accelerations.clear();
00165   joint_state->time_from_start = ros::Duration(0);
00166 
00167   return true;
00168 }
00169 
00170 bool JointRelayHandler::select(const JointTrajectoryPoint& all_joint_state, const std::vector<std::string>& all_joint_names,
00171                                JointTrajectoryPoint* pub_joint_state, std::vector<std::string>* pub_joint_names)
00172 {
00173   ROS_ASSERT(all_joint_state.positions.size() == all_joint_names.size());
00174 
00175   *pub_joint_state = JointTrajectoryPoint();  // start with a "clean" message
00176   pub_joint_names->clear();
00177 
00178   // skip over "blank" joint names
00179   for (int i=0; i<all_joint_names.size(); ++i)
00180   {
00181     if (all_joint_names[i].empty())
00182       continue;
00183 
00184     pub_joint_names->push_back(all_joint_names[i]);
00185     if (!all_joint_state.positions.empty())
00186       pub_joint_state->positions.push_back(all_joint_state.positions[i]);
00187     if (!all_joint_state.velocities.empty())
00188       pub_joint_state->velocities.push_back(all_joint_state.velocities[i]);
00189     if (!all_joint_state.accelerations.empty())
00190       pub_joint_state->accelerations.push_back(all_joint_state.accelerations[i]);
00191   }
00192   pub_joint_state->time_from_start = all_joint_state.time_from_start;
00193 
00194   return true;
00195 }
00196 
00197 }//namespace joint_relay_handler
00198 }//namespace industrial_robot_client
00199 
00200 
00201 
00202 
 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