joy_relative_converter.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import rospy
00003 
00004 try:
00005   from sensor_msgs.msg import Joy, JoyFeedbackArray
00006 except:
00007   roslib.load_manifest('jsk_teleop_joy')
00008   from sensor_msgs.msg import Joy, JoyFeedbackArray
00009 from std_msgs.msg import Int8, Empty
00010 
00011 class AxesConverter:
00012   def __init__(self):
00013     self.input_origins = []
00014     self.output_origins = []
00015     self.prev_output_axes = [i for i in self.output_origins]
00016 
00017   def adjustAxesSize(self, axes_size):
00018     if len(self.input_origins) != axes_size:
00019       if len(self.input_origins) == 0:
00020         self.input_origins = [None] * axes_size
00021         self.output_origins = [0.5] * axes_size
00022         rospy.loginfo("[midi_relative_converter] adjust axes size.")
00023       else:
00024         rospy.logwarn("[midi_relative_converter] different axes size!!")
00025 
00026   def convert(self, abs_axes):
00027     self.adjustAxesSize(len(abs_axes))
00028     self.updateInputOrigins(abs_axes)
00029     rel_axes = self.__convertAxes(abs_axes, self.input_origins, self.output_origins)
00030     self.__updatePrevOutputAxes(rel_axes)
00031     return rel_axes
00032 
00033   def resetAllOrigins(self):
00034     self.input_origins = []
00035     self.output_origins = []
00036     self.prev_output_axes = [i for i in self.output_origins]
00037 
00038   def resetInputOrigins(self):
00039     for i in range(len(self.input_origins)):
00040       self.input_origins[i] = None
00041 
00042   def updateInputOrigins(self, abs_axes):
00043     for i in range(len(abs_axes)):
00044       if self.input_origins[i] is None and abs_axes[i] != 0:
00045         self.input_origins[i] = abs_axes[i]
00046 
00047   def __updatePrevOutputAxes(self, rel_axes):
00048     self.prev_output_axes = list(rel_axes)
00049 
00050   def updateOutputOrigins(self):
00051     self.output_origins = list(self.prev_output_axes)
00052 
00053   def __convertAxes(self, abs_axes, in_origins, out_origins):
00054     rel_axes = [None] * len(abs_axes)
00055     for i in range(len(abs_axes)):
00056       rel_axes[i] = self.__convertAxis(abs_axes[i], in_origins[i], out_origins[i])
00057     return rel_axes
00058 
00059   def __convertAxis(self, abs_axis, in_origin, out_origin):
00060     if abs_axis == 1:
00061       rel_axis = abs_axis
00062     elif in_origin is None:
00063       rel_axis = out_origin
00064     elif abs_axis < in_origin:
00065       rel_axis = (((abs_axis - 0.0) /  (in_origin - 0.0)) * (out_origin - 0.0)) + 0.0
00066     elif abs_axis == in_origin:
00067       rel_axis = out_origin
00068     elif abs_axis > in_origin:
00069       rel_axis = (((abs_axis - in_origin) /  (1.0 - in_origin)) * (1.0 - out_origin)) + out_origin
00070     return rel_axis
00071 
00072 class AxesConverterArray:
00073   def __init__(self, max_page=1):
00074     self.ac = [AxesConverter() for i in range(max_page)]
00075     self.crt_page = 0
00076     self.prev_input_axes = []
00077     for page in range(max_page):
00078       joy_pub = rospy.Publisher("/joy_relative/page_" + str(page), Joy)
00079     rospy.Subscriber("/joy", Joy, self.joy_callback)
00080     rospy.Subscriber("/midi_relative_converter/command/switch_page", Int8, self.switch_page_cmd_cb)
00081     rospy.Subscriber("/midi_relative_converter/command/reset", Empty, self.reset_cmd_cb)
00082 
00083   def convert(self, abs_axes):
00084     return self.ac[self.crt_page].convert(abs_axes)
00085 
00086   def switchPage(self, page):
00087     if page >= 0 and page < len(self.ac):
00088       if page != self.crt_page:
00089         self.__updateReserveInputOrigins(self.prev_input_axes)
00090         self.__updateCurrentOutputOrigins()
00091         self.crt_page = page
00092       return page
00093     else:
00094       return -1
00095 
00096   def resetAllOrigins(self):
00097     for i in range(len(self.ac)):
00098       self.ac[i].resetAllOrigins()
00099 
00100   def __updateReserveInputOrigins(self, abs_axes):
00101     for i in range(len(self.ac)):
00102       if i != self.crt_page:
00103         self.ac[i].adjustAxesSize(len(abs_axes))
00104         self.ac[i].resetInputOrigins()
00105         self.ac[i].updateInputOrigins(abs_axes)
00106 
00107   def __updateCurrentOutputOrigins(self):
00108     self.ac[self.crt_page].updateOutputOrigins()
00109 
00110   def joy_callback(self, data):
00111     joy = Joy()
00112     joy.header.stamp = rospy.Time.now()
00113     joy.axes = self.convert(data.axes)
00114     joy.buttons = data.buttons
00115     self.prev_input_axes = list(data.axes)
00116     joy_pub = rospy.Publisher("/joy_relative/page_" + str(self.crt_page), Joy)
00117     joy_pub.publish(joy)
00118 
00119   def switch_page_cmd_cb(self, msg):
00120     self.switchPage(msg.data)
00121 
00122   def reset_cmd_cb(self, msg):
00123     self.resetAllOrigins()
00124 
00125 def main():
00126   rospy.init_node('joy_relative_converter')
00127   aca = AxesConverterArray(3)
00128   rospy.spin()
00129 
00130 if __name__ == '__main__':
00131   main()


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Fri Apr 19 2019 03:45:43