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 "industrial_robot_client/joint_relay_handler.h"
00035 #include "simple_message/log_wrapper.h"
00036 
00037 using industrial::shared_types::shared_real;
00038 using industrial::smpl_msg_connection::SmplMsgConnection;
00039 using namespace industrial::simple_message;
00040 
00041 namespace industrial_robot_client
00042 {
00043 namespace joint_relay_handler
00044 {
00045 
00046 bool JointRelayHandler::init(SmplMsgConnection* connection, std::vector<std::string>& joint_names)
00047 {
00048   this->pub_joint_control_state_ =
00049           this->node_.advertise<control_msgs::FollowJointTrajectoryFeedback>("feedback_states", 1);
00050 
00051   this->pub_joint_sensor_state_ = this->node_.advertise<sensor_msgs::JointState>("joint_states",1);
00052 
00053   // save "complete" joint-name list, preserving any blank entries for later use
00054   this->all_joint_names_ = joint_names;
00055 
00056   return init((int)StandardMsgTypes::JOINT, connection);
00057 }
00058 
00059 bool JointRelayHandler::internalCB(SimpleMessage& in)
00060 {
00061   JointMessage joint_msg;
00062 
00063   if (!joint_msg.init(in))
00064   {
00065     LOG_ERROR("Failed to initialize joint message");
00066     return false;
00067   }
00068 
00069   return internalCB(joint_msg);
00070 }
00071 
00072 bool JointRelayHandler::internalCB(JointMessage& in)
00073 {
00074   control_msgs::FollowJointTrajectoryFeedback control_state;
00075   sensor_msgs::JointState sensor_state;
00076   bool rtn = true;
00077 
00078   if (create_messages(in, &control_state, &sensor_state))
00079   {
00080     this->pub_joint_control_state_.publish(control_state);
00081     this->pub_joint_sensor_state_.publish(sensor_state);
00082   }
00083   else
00084     rtn = false;
00085 
00086   // Reply back to the controller if the sender requested it.
00087   if (CommTypes::SERVICE_REQUEST == in.getMessageType())
00088   {
00089     SimpleMessage reply;
00090     in.toReply(reply, rtn ? ReplyTypes::SUCCESS : ReplyTypes::FAILURE);
00091     this->getConnection()->sendMsg(reply);
00092   }
00093 
00094   return rtn;
00095 }
00096 
00097 // TODO: Add support for other message fields (velocity, effort, desired pos)
00098 bool JointRelayHandler::create_messages(JointMessage& msg_in,
00099                                         control_msgs::FollowJointTrajectoryFeedback* control_state,
00100                                         sensor_msgs::JointState* sensor_state)
00101 {
00102   // read joint positions from JointMessage
00103   std::vector<double> all_joint_pos(all_joint_names_.size());
00104   for (int i=0; i<all_joint_names_.size(); ++i)
00105   {
00106     shared_real value;
00107     if (msg_in.getJoints().getJoint(i, value))
00108       all_joint_pos[i] = value;
00109     else
00110       LOG_ERROR("Failed to parse #%d value from JointMessage", i);
00111   }
00112 
00113   // apply transform to joint positions, if required
00114   std::vector<double> xform_joint_pos;
00115   if (!transform(all_joint_pos, &xform_joint_pos))
00116   {
00117     LOG_ERROR("Failed to transform joint positions");
00118     return false;
00119   }
00120 
00121   // select specific joints for publishing
00122   std::vector<double> pub_joint_pos;
00123   std::vector<std::string> pub_joint_names;
00124   if (!select(xform_joint_pos, all_joint_names_, &pub_joint_pos, &pub_joint_names))
00125   {
00126     LOG_ERROR("Failed to select joints for publishing");
00127     return false;
00128   }
00129 
00130   // assign values to messages
00131   control_msgs::FollowJointTrajectoryFeedback tmp_control_state;  // always start with a "clean" message
00132   tmp_control_state.header.stamp = ros::Time::now();
00133   tmp_control_state.joint_names = pub_joint_names;
00134   tmp_control_state.actual.positions = pub_joint_pos;
00135   *control_state = tmp_control_state;
00136 
00137   sensor_msgs::JointState tmp_sensor_state;
00138   tmp_sensor_state.header.stamp = ros::Time::now();
00139   tmp_sensor_state.name = pub_joint_names;
00140   tmp_sensor_state.position = pub_joint_pos;
00141   *sensor_state = tmp_sensor_state;
00142 
00143   return true;
00144 }
00145 
00146 bool JointRelayHandler::select(const std::vector<double>& all_joint_pos, const std::vector<std::string>& all_joint_names,
00147             std::vector<double>* pub_joint_pos, std::vector<std::string>* pub_joint_names)
00148 {
00149   ROS_ASSERT(all_joint_pos.size() == all_joint_names.size());
00150 
00151   pub_joint_pos->clear();
00152   pub_joint_names->clear();
00153 
00154   // skip over "blank" joint names
00155   for (int i=0; i<all_joint_pos.size(); ++i)
00156   {
00157     if (all_joint_names[i].empty())
00158       continue;
00159 
00160     pub_joint_pos->push_back(all_joint_pos[i]);
00161     pub_joint_names->push_back(all_joint_names[i]);
00162   }
00163 
00164   return true;
00165 }
00166 
00167 }//namespace joint_relay_handler
00168 }//namespace industrial_robot_client
00169 
00170 
00171 
00172 


industrial_robot_client
Author(s): Jeremy Zoss
autogenerated on Fri Aug 28 2015 11:12:09