joint_states_aggregator.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #################################################################
00004 ##\file
00005 #
00006 # \note
00007 # Copyright (c) 2012 \n
00008 # Fraunhofer Institute for Manufacturing Engineering
00009 # and Automation (IPA) \n\n
00010 #
00011 #################################################################
00012 #
00013 # \note
00014 # Project name: Care-O-bot Research
00015 # \note
00016 # ROS package name: 
00017 #
00018 # \author
00019 # Author: Thiago de Freitas Oliveira Araujo, 
00020 # email:thiago.de.freitas.oliveira.araujo@ipa.fhg.de
00021 # \author
00022 # Supervised by: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de
00023 #
00024 # \date Date of creation: December 2012
00025 #
00026 # \brief
00027 # This module aggregates joint states
00028 #
00029 #################################################################
00030 #
00031 # Redistribution and use in source and binary forms, with or without
00032 # modification, are permitted provided that the following conditions are met:
00033 #
00034 # - Redistributions of source code must retain the above copyright
00035 # notice, this list of conditions and the following disclaimer. \n
00036 # - Redistributions in binary form must reproduce the above copyright
00037 # notice, this list of conditions and the following disclaimer in the
00038 # documentation and/or other materials provided with the distribution. \n
00039 # - Neither the name of the Fraunhofer Institute for Manufacturing
00040 # Engineering and Automation (IPA) nor the names of its
00041 # contributors may be used to endorse or promote products derived from
00042 # this software without specific prior written permission. \n
00043 #
00044 # This program is free software: you can redistribute it and/or modify
00045 # it under the terms of the GNU Lesser General Public License LGPL as
00046 # published by the Free Software Foundation, either version 3 of the
00047 # License, or (at your option) any later version.
00048 #
00049 # This program is distributed in the hope that it will be useful,
00050 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00051 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00052 # GNU Lesser General Public License LGPL for more details.
00053 #
00054 # You should have received a copy of the GNU Lesser General Public
00055 # License LGPL along with this program.
00056 # If not, see < http://www.gnu.org/licenses/>.
00057 #
00058 #################################################################
00059 import roslib
00060 roslib.load_manifest('srs_scenarios')
00061 import rospy
00062 
00063 import rosbag
00064 from std_msgs.msg import Int32, String
00065 
00066 import tf
00067 from tf.msg import *
00068 from tf.transformations import euler_from_quaternion
00069 
00070 from simple_script_server import *
00071 sss = simple_script_server()
00072 
00073 from kinematics_msgs.srv import *
00074 from std_msgs.msg import *
00075 from sensor_msgs.msg import *
00076 from move_base_msgs.msg import *
00077 
00078 import math
00079 import rostopic
00080 
00081 import os
00082 import sys, subprocess
00083 
00084 import itertools
00085 
00086 import threading
00087 
00088 class joint_state_aggregator():
00089 
00090     def __init__(self):
00091         # lock object is necessary to prevent interruption on the joint_states message
00092         # update process
00093         self.lock = threading.Lock()
00094         # the following lists store the components of the joint_states message
00095         self.effort = []
00096         self.position = []
00097         self.velocity = []
00098         self.names = []
00099         # joint_states message
00100         self.jointsMsg = rospy.wait_for_message("/joint_states", sensor_msgs.msg.JointState, 10)
00101         # this checks for the dimensions of the message to inform if there is a 
00102         # malformed joint_states message for the developer to be able to correct
00103         # the related controller code
00104         if(len(self.jointsMsg.name) == len(self.jointsMsg.position) == len(self.jointsMsg.velocity) == len(self.jointsMsg.effort)):
00105             rospy.logdebug("Dimensions of joint_state message are ok!")
00106         else:
00107             excep = "Joint_states dimensions are not correct:" + "Names_dim " + (str)(len(self.jointsMsg.name)) + "," + "Pos_dim " + (str)(len(self.jointsMsg.position)) + "," + "Vel_dim " + (str)(len(self.jointsMsg.velocity)) + "," + "Eff_dim " + (str)(len(self.jointsMsg.effort))
00108             raise Exception(excep)
00109         # iterates simultaneously through all the message lists to update the correspondent
00110         # values according to the new joint_states message
00111         for a,b,c, d in itertools.izip(self.jointsMsg.name, self.jointsMsg.position,self.jointsMsg.velocity, self.jointsMsg.effort):
00112             self.names.append(a)
00113             self.velocity.append(b)
00114             self.position.append(c)
00115             self.effort.append(d)
00116         # establishes a subscriber and a callback to the joint_states
00117         rospy.Subscriber('/joint_states', sensor_msgs.msg.JointState, self.process_joints)
00118         # creates a thread for managing the joint_states messages listener
00119         #self.thread = threading.Thread(target=self.joint_states_listener)
00120         #self.thread.start()
00121         
00122     def joint_states_listener(self):
00123         rospy.Subscriber('/joint_states', JointState, self.process_joints)
00124         rospy.spin()
00125         
00126     def process_joints(self, msg):
00127         self.lock.acquire()
00128         jMsg = msg#rospy.wait_for_message("/joint_states", sensor_msgs.msg.JointState, 10)
00129         self.temp_name = jMsg.name
00130         self.temp_vel = jMsg.velocity
00131         self.temp_pos = jMsg.position
00132         self.temp_ef = jMsg.effort
00133         # dimensions check
00134         if(len(jMsg.name) == len(jMsg.position) == len(jMsg.velocity) == len(jMsg.effort)):
00135             pass
00136         else:
00137             excep = "Joint_states dimensions are not correct:" + "Names_dim " + (str)(len(jMsg.name)) + "," + "Pos_dim " + (str)(len(jMsg.position)) + "," + "Vel_dim " + (str)(len(jMsg.velocity)) + "," + "Eff_dim " + (str)(len(jMsg.effort)) + "joint_names = " + str(jMsg.name)
00138             raise Exception(excep)
00139         # iterates through the message items to update the corresponding lists
00140         for a,b,c, d in itertools.izip(jMsg.name, jMsg.position,jMsg.velocity, jMsg.effort):
00141         
00142             if(a) not in self.names:
00143                 self.names.append(a)
00144                 self.position.append(b)
00145                 self.velocity.append(c)
00146                 self.effort.append(d)
00147             
00148             # this is important for updating the current items that are already
00149             # on the lists
00150             self.position[self.names.index(a)] = self.temp_pos[self.temp_name.index(a)]
00151             self.velocity[self.names.index(a)] = self.temp_vel[self.temp_name.index(a)]
00152             self.effort[self.names.index(a)] = self.temp_ef[self.temp_name.index(a)]
00153         # this creates the aggregated joint_states message and makes it available
00154         # when requested
00155         self.jointsMsg.name = self.names
00156         self.jointsMsg.position = self.position
00157         self.jointsMsg.velocity = self.velocity
00158         self.jointsMsg.effort = self.effort
00159         
00160         self.lock.release()
00161         #return self.jointsMsg
00162         
00163 if __name__=="__main__":
00164 
00165     rospy.init_node('joint_test')
00166     ja = joint_state_aggregator()
00167     rate = rospy.Rate(10) #Hz
00168     
00169     while not rospy.is_shutdown():
00170         
00171         #ja.process_joints()
00172         
00173         rospy.loginfo("Current message:")
00174         rospy.loginfo(ja.jointsMsg)
00175         rospy.loginfo("Collected names:")
00176         rospy.loginfo(ja.names)
00177         rospy.loginfo(ja.position)
00178         rospy.loginfo(ja.velocity)
00179         
00180         rate.sleep()


srs_user_tests
Author(s): SRS team
autogenerated on Mon Oct 6 2014 08:53:11