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 <dx100/joint_relay_handler.h>
00033 #include "simple_message/messages/joint_message.h"
00034 #include "simple_message/log_wrapper.h"
00035 
00036 
00037 using namespace industrial::joint_message;
00038 using namespace industrial::simple_message;
00039 using namespace industrial::shared_types;
00040 
00041 namespace motoman
00042 {
00043 namespace joint_relay_handler
00044 {
00045 
00046 JointRelayHandler::JointRelayHandler(ros::NodeHandle &n) :
00047         joint_control_state_(),
00048         node_(n)
00049 {
00050 
00051   this->pub_joint_control_state_ =
00052           this->node_.advertise<control_msgs::FollowJointTrajectoryFeedback>("feedback_states", 1);
00053 
00054   this->pub_joint_sensor_state_ = this->node_.advertise<sensor_msgs::JointState>("joint_states",1);
00055 
00056   // Set up the default joint names (TODO: This should be made more generic.  The
00057   // joint names can have an arbitrary prefix that isn't accounted for here.  This
00058   // info can be found in the URDF, but I don't know how to get it here.
00059   this->joint_control_state_.joint_names.push_back("joint_s");
00060   this->joint_control_state_.joint_names.push_back("joint_l");
00061   this->joint_control_state_.joint_names.push_back("joint_e");
00062   this->joint_control_state_.joint_names.push_back("joint_u");
00063   this->joint_control_state_.joint_names.push_back("joint_r");
00064   this->joint_control_state_.joint_names.push_back("joint_b");
00065   this->joint_control_state_.joint_names.push_back("joint_t");
00066   // TODO: Again, this should be more generic.
00067   this->joint_control_state_.actual.positions.resize(NUM_OF_JOINTS_);
00068   this->joint_control_state_.desired.positions.resize(NUM_OF_JOINTS_);
00069   this->joint_control_state_.error.positions.resize(NUM_OF_JOINTS_);
00070 
00071   // Copy from control state to sensor state
00072   this->joint_sensor_state_.name = this->joint_control_state_.joint_names;
00073   this->joint_sensor_state_.position.resize(NUM_OF_JOINTS_);
00074   this->joint_sensor_state_.velocity.resize(NUM_OF_JOINTS_);
00075   this->joint_sensor_state_.effort.resize(NUM_OF_JOINTS_);
00076 }
00077 
00078 bool JointRelayHandler::init(industrial::smpl_msg_connection::SmplMsgConnection* connection)
00079 {
00080   return this->init(StandardMsgTypes::JOINT, connection);
00081 }
00082 
00083 bool JointRelayHandler::internalCB(industrial::simple_message::SimpleMessage & in)
00084 {
00085   bool rtn = false;
00086   JointMessage joint;
00087   SimpleMessage msg;
00088 
00089   if (joint.init(in))
00090   {
00091     shared_real value;
00092     for(int i =0; i < NUM_OF_JOINTS_; i++)
00093     {
00094       if (joint.getJoints().getJoint(i, value))
00095       {
00096         this->joint_control_state_.actual.positions[i] = value;
00097         this->joint_sensor_state_.position[i] = value;
00098       }
00099       else
00100       {
00101         this->joint_control_state_.actual.positions[i] = 0.0;
00102         LOG_ERROR("Failed to populate ith(%d) of controller state message", i);
00103       }
00104       // TODO: For now these values are not populated
00105       this->joint_control_state_.desired.positions[i] = 0.0;
00106       this->joint_control_state_.error.positions[i] = 0.0;
00107     }
00108     this->joint_control_state_.header.stamp = ros::Time::now();
00109     this->pub_joint_control_state_.publish(this->joint_control_state_);
00110 
00111     this->joint_sensor_state_.header.stamp = ros::Time::now();
00112     this->pub_joint_sensor_state_.publish(this->joint_sensor_state_);
00113 
00114     // Reply back to the controller if the sender requested it.
00115     if (CommTypes::SERVICE_REQUEST == in.getMessageType())
00116     {
00117       joint.toReply(msg, ReplyTypes::SUCCESS);
00118     }
00119   }
00120   else
00121   {
00122     LOG_ERROR("Failed to initialize joint message");
00123     rtn = false;
00124   }
00125 
00126   return rtn;
00127 }
00128 
00129 
00130 
00131 }//namespace ping_handler
00132 }//namespace industrial
00133 
00134 
00135 
00136 


dx100
Author(s): Shaun Edwards (Southwest Research Institute)
autogenerated on Mon Oct 6 2014 02:25:33