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