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 "adept_common/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 adept
00042 {
00043 namespace joint_relay_handler
00044 {
00045 
00046 JointRelayHandler::JointRelayHandler(ros::NodeHandle &n) :
00047         node_(n),
00048         joint_control_state_()
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_1");
00060   this->joint_control_state_.joint_names.push_back("joint_2");
00061   this->joint_control_state_.joint_names.push_back("joint_3");
00062   this->joint_control_state_.joint_names.push_back("joint_4");
00063   this->joint_control_state_.joint_names.push_back("joint_5");
00064   this->joint_control_state_.joint_names.push_back("joint_6");
00065   // TODO: Again, this should be more generic.
00066   this->joint_control_state_.actual.positions.resize(NUM_OF_JOINTS_);
00067   this->joint_control_state_.desired.positions.resize(NUM_OF_JOINTS_);
00068   this->joint_control_state_.error.positions.resize(NUM_OF_JOINTS_);
00069 
00070   // Copy from control state to sensor state
00071   this->joint_sensor_state_.name = this->joint_control_state_.joint_names;
00072   this->joint_sensor_state_.position.resize(NUM_OF_JOINTS_);
00073   this->joint_sensor_state_.velocity.resize(NUM_OF_JOINTS_);
00074   this->joint_sensor_state_.effort.resize(NUM_OF_JOINTS_);
00075 }
00076 
00077 bool JointRelayHandler::init(industrial::smpl_msg_connection::SmplMsgConnection* connection)
00078 {
00079   return this->init(StandardMsgTypes::JOINT, connection);
00080 }
00081 
00082 bool JointRelayHandler::internalCB(industrial::simple_message::SimpleMessage & in)
00083 {
00084   bool rtn = false;
00085   JointMessage joint;
00086   SimpleMessage msg;
00087 
00088   LOG_INFO("Executing internal CB");
00089 
00090   if (joint.init(in))
00091   {
00092     shared_real value;
00093     for(int i =0; i < NUM_OF_JOINTS_; i++)
00094     {
00095       if (joint.getJoints().getJoint(i, value))
00096       {
00097         this->joint_control_state_.actual.positions[i] = value;
00098         this->joint_sensor_state_.position[i] = value;
00099       }
00100       else
00101       {
00102         this->joint_control_state_.actual.positions[i] = 0.0;
00103         LOG_ERROR("Failed to populate ith(%d) of controller state message", i);
00104       }
00105       // TODO: For now these values are not populated
00106       this->joint_control_state_.desired.positions[i] = 0.0;
00107       this->joint_control_state_.error.positions[i] = 0.0;
00108     }
00109     this->joint_control_state_.header.stamp = ros::Time::now();
00110     this->pub_joint_control_state_.publish(this->joint_control_state_);
00111 
00112     this->joint_sensor_state_.header.stamp = ros::Time::now();
00113     this->pub_joint_sensor_state_.publish(this->joint_sensor_state_);
00114 
00115     // Reply back to the controller if the sender requested it.
00116     if (CommTypes::SERVICE_REQUEST == in.getMessageType())
00117     {
00118       joint.toReply(msg, ReplyTypes::SUCCESS);
00119     }
00120   }
00121   else
00122   {
00123     LOG_ERROR("Failed to initialize joint message");
00124     rtn = false;
00125   }
00126 
00127   return rtn;
00128 }
00129 
00130 
00131 
00132 }//namespace ping_handler
00133 }//namespace industrial
00134 
00135 
00136 
00137 


adept_common
Author(s): root
autogenerated on Fri Jan 3 2014 11:05:38