joint_states_appender.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #####################################################################
00003 # Software License Agreement (BSD License)
00004 #
00005 #  Copyright (c) 2015, JSK Lab
00006 #  All rights reserved.
00007 #
00008 #  Redistribution and use in source and binary forms, with or without
00009 #  modification, are permitted provided that the following conditions
00010 #  are met:
00011 #
00012 #   * Redistributions of source code must retain the above copyright
00013 #     notice, this list of conditions and the following disclaimer.
00014 #   * Redistributions in binary form must reproduce the above
00015 #     copyright notice, this list of conditions and the following
00016 #     disclaimer in the documentation and/o2r other materials provided
00017 #     with the distribution.
00018 #   * Neither the name of the JSK Lab nor the names of its
00019 #     contributors may be used to endorse or promote products derived
00020 #     from this software without specific prior written permission.
00021 #
00022 #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 #  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 #  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 #  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 #  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 #  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 #  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 #  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 #  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 #  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 #  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 #  POSSIBILITY OF SUCH DAMAGE.
00034 #####################################################################
00035 
00036 
00037 import rospy
00038 from sensor_msgs.msg import JointState
00039 
00040 prev_joint_states = None
00041 pub = None
00042 
00043 def callback(msg):
00044     global prev_joint_states, pub
00045     if not prev_joint_states:
00046         # initial time
00047         # just pass the input message to output
00048         pub.publish(msg)
00049         prev_joint_states = msg
00050         # force to use list instead of tuple.
00051         prev_joint_states.name = list(prev_joint_states.name)
00052         prev_joint_states.position = list(prev_joint_states.position)
00053         if len(prev_joint_states.velocity) > 0:
00054             prev_joint_states.velocity = list(prev_joint_states.velocity)
00055         else:
00056             prev_joint_states.velocity = [0] * len(prev_joint_states.name)
00057         if len(prev_joint_states.effort) > 0:
00058             prev_joint_states.effort = list(prev_joint_states.effort)
00059         else:
00060             prev_joint_states.effort = [0] * len(prev_joint_states.name)
00061     else:
00062         if len(msg.velocity) > 0:
00063             velocity = msg.velocity
00064         else:
00065             velocity = [0] * len(msg.name)
00066         if len(msg.effort) > 0:
00067             effort = msg.effort
00068         else:
00069             effort = [0] * len(msg.name)
00070 
00071         for name, pos, vel, ef in zip(msg.name, msg.position, velocity, effort):
00072             if name in prev_joint_states.name:
00073                 # if name is already listed in prev_joint_states,
00074                 # update value
00075                 index = prev_joint_states.name.index(name)
00076                 prev_joint_states.position[index] = pos
00077                 prev_joint_states.velocity[index] = vel
00078                 prev_joint_states.effort[index] = ef
00079             else:
00080                 # if name is not yet listed in prev_joint_states,
00081                 # append name and position to prev_joint_states
00082                 prev_joint_states.name.append(name)
00083                 prev_joint_states.position.append(pos)
00084                 prev_joint_states.effort.append(ef)
00085                 prev_joint_states.velocity.append(vel)
00086         if prev_joint_states.header.stamp < msg.header.stamp:
00087             prev_joint_states.header.stamp = msg.header.stamp
00088         pub.publish(prev_joint_states)
00089         
00090 if __name__ == "__main__":
00091     rospy.init_node("joint_states_appender")
00092     pub = rospy.Publisher("joint_states_appended", JointState)
00093     sub = rospy.Subscriber("joint_states", JointState, callback, queue_size=1)
00094     rospy.spin()


jsk_calibration
Author(s):
autogenerated on Wed Sep 16 2015 04:37:40