Created
December 9, 2015 12:01
-
-
Save awesomebytes/236bf500433eb6d29a1c to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| """ | |
| Licensed under the Apache License, Version 2.0 (the "License"); | |
| you may not use this file except in compliance with the License. | |
| You may obtain a copy of the License at | |
| http://www.apache.org/licenses/LICENSE-2.0 | |
| Unless required by applicable law or agreed to in writing, software | |
| distributed under the License is distributed on an "AS IS" BASIS, | |
| WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | |
| See the License for the specific language governing permissions and | |
| limitations under the License. | |
| """ | |
| import remi.gui as gui | |
| from remi import start, App | |
| import rospy | |
| from sensor_msgs.msg import JointState | |
| import xml.dom.minidom | |
| from math import pi | |
| def get_joint_limits(key='robot_description', use_smallest_joint_limits=True): | |
| use_small = use_smallest_joint_limits | |
| use_mimic = True | |
| # Code inspired on the joint_state_publisher package by David Lu!!! | |
| # https://github.com/ros/robot_model/blob/indigo-devel/ | |
| # joint_state_publisher/joint_state_publisher/joint_state_publisher | |
| description = rospy.get_param(key) | |
| robot = xml.dom.minidom.parseString(description)\ | |
| .getElementsByTagName('robot')[0] | |
| free_joints = {} | |
| dependent_joints = {} | |
| # Find all non-fixed joints | |
| for child in robot.childNodes: | |
| if child.nodeType is child.TEXT_NODE: | |
| continue | |
| if child.localName == 'joint': | |
| jtype = child.getAttribute('type') | |
| if jtype == 'fixed': | |
| continue | |
| name = child.getAttribute('name') | |
| if jtype == 'continuous': | |
| minval = -pi | |
| maxval = pi | |
| else: | |
| try: | |
| limit = child.getElementsByTagName('limit')[0] | |
| minval = float(limit.getAttribute('lower')) | |
| maxval = float(limit.getAttribute('upper')) | |
| except: | |
| continue | |
| try: | |
| maxvel = float(limit.getAttribute('velocity')) | |
| except: | |
| continue | |
| safety_tags = child.getElementsByTagName('safety_controller') | |
| if use_small and len(safety_tags) == 1: | |
| tag = safety_tags[0] | |
| if tag.hasAttribute('soft_lower_limit'): | |
| minval = max(minval, | |
| float(tag.getAttribute('soft_lower_limit'))) | |
| if tag.hasAttribute('soft_upper_limit'): | |
| maxval = min(maxval, | |
| float(tag.getAttribute('soft_upper_limit'))) | |
| mimic_tags = child.getElementsByTagName('mimic') | |
| if use_mimic and len(mimic_tags) == 1: | |
| tag = mimic_tags[0] | |
| entry = {'parent': tag.getAttribute('joint')} | |
| if tag.hasAttribute('multiplier'): | |
| entry['factor'] = float(tag.getAttribute('multiplier')) | |
| if tag.hasAttribute('offset'): | |
| entry['offset'] = float(tag.getAttribute('offset')) | |
| dependent_joints[name] = entry | |
| continue | |
| if name in dependent_joints: | |
| continue | |
| joint = {'min_position': minval, 'max_position': maxval} | |
| joint["has_position_limits"] = jtype != 'continuous' | |
| joint['max_velocity'] = maxvel | |
| free_joints[name] = joint | |
| return free_joints | |
| class SvgPlot(gui.Svg): | |
| def __init__(self, width, height): | |
| super(SvgPlot, self).__init__(width, height) | |
| self.width = width | |
| self.height = height | |
| self.polyList = [] | |
| self.font_size = 15 | |
| self.plot_inner_border = self.font_size | |
| self.textYMin = gui.SvgText(0,self.height + self.font_size, "min") | |
| self.textYMax = gui.SvgText(0,0, "max") | |
| self.textYMin.style['font-size'] = gui.to_pix(self.font_size) | |
| self.textYMax.style['font-size'] = gui.to_pix(self.font_size) | |
| self.append( str(id(self.textYMin)), self.textYMin ) | |
| self.append( str(id(self.textYMax)), self.textYMax ) | |
| def append_poly(self, poly): | |
| self.append(str(id(poly)), poly) | |
| self.polyList.append(poly) | |
| poly.textXMin = gui.SvgText(0,0,"actualValue") | |
| poly.textXMax = gui.SvgText(0,0,"actualValue") | |
| poly.textYVal = gui.SvgText(0,0,"actualValue") | |
| poly.textYVal.style['font-size'] = gui.to_pix(self.font_size) | |
| poly.lineYValIndicator = gui.SvgLine(0,0,0,0) | |
| poly.lineXMinIndicator = gui.SvgLine(0,0,0,0) | |
| poly.lineXMaxIndicator = gui.SvgLine(0,0,0,0) | |
| self.append( str(id(poly.textXMin)), poly.textXMin ) | |
| self.append( str(id(poly.textXMax)), poly.textXMax ) | |
| self.append( str(id(poly.textYVal)), poly.textYVal ) | |
| self.append( str(id(poly.lineYValIndicator)), poly.lineYValIndicator ) | |
| self.append( str(id(poly.lineXMinIndicator)), poly.lineXMinIndicator ) | |
| self.append( str(id(poly.lineXMaxIndicator)), poly.lineXMaxIndicator ) | |
| def remove_poly(self, poly): | |
| self.remove(poly) | |
| self.polyList.remove(poly) | |
| self.remove(poly.textXMin) | |
| self.remove(poly.textXMax) | |
| self.remove(poly.textYVal) | |
| def render(self, fixed_minY=None, fixed_maxY=None): | |
| self.set_viewbox(-self.plot_inner_border, -self.plot_inner_border, self.width + self.plot_inner_border*2, self.height + self.plot_inner_border*2) | |
| if len(self.polyList)<1: | |
| return | |
| minX = min(self.polyList[0].coordsX) | |
| maxX = max(self.polyList[0].coordsX) | |
| if fixed_minY is None: | |
| minY = min(self.polyList[0].coordsY) | |
| else: | |
| minY = fixed_minY | |
| if fixed_maxY is None: | |
| maxY = max(self.polyList[0].coordsY) | |
| else: | |
| maxY = fixed_maxY | |
| for poly in self.polyList: | |
| minX = min(minX, min(poly.coordsX)) | |
| maxX = max(maxX, max(poly.coordsX)) | |
| if fixed_minY is None: | |
| minY = min(minY, min(poly.coordsY)) | |
| if fixed_maxY is None: | |
| maxY = max(maxY, max(poly.coordsY)) | |
| self.textYMin.set_text( "min:%s"%minY ) | |
| self.textYMax.set_text( "max:%s"%maxY ) | |
| scaleWidth = 1.0 | |
| scaleHeight = 1.0 | |
| if (maxX > minX): | |
| scaleWidth = self.width/float(abs(maxX-minX)) | |
| if (maxY > minY): | |
| scaleHeight = self.height/float(abs(maxY-minY)) | |
| i = 1 | |
| for poly in self.polyList: | |
| scaledTranslatedYpos = (-poly.coordsY[-1]+maxY)*scaleHeight | |
| textXpos = self.height/(len(self.polyList)+1)*i | |
| poly.textXMin.set_text(str(min(poly.coordsX))) | |
| poly.textXMin.set_fill(poly.style['stroke']) | |
| poly.textXMin.set_position(-textXpos, (min(poly.coordsX)-minX) * scaleWidth) | |
| poly.textXMin.attributes['transform'] = "rotate(%s)" % (-90) | |
| poly.textXMax.set_text(str(max(poly.coordsX))) | |
| poly.textXMax.set_fill(poly.style['stroke']) | |
| poly.textXMax.set_position(-textXpos, (max(poly.coordsX)-minX) * scaleWidth) | |
| poly.textXMax.attributes['transform'] = "rotate(%s)" % (-90) | |
| poly.textYVal.set_text(str(poly.coordsY[-1])) | |
| poly.textYVal.set_fill(poly.style['stroke']) | |
| poly.textYVal.set_position(0, scaledTranslatedYpos) | |
| poly.lineYValIndicator.set_stroke(1, poly.style['stroke']) | |
| poly.lineXMinIndicator.set_stroke(1, poly.style['stroke']) | |
| poly.lineXMaxIndicator.set_stroke(1, poly.style['stroke']) | |
| poly.lineYValIndicator.set_coords(0, scaledTranslatedYpos, self.width, scaledTranslatedYpos) | |
| poly.lineXMinIndicator.set_coords((min(poly.coordsX)-minX) * scaleWidth, 0, (min(poly.coordsX)-minX) * scaleWidth, self.height) | |
| poly.lineXMaxIndicator.set_coords((max(poly.coordsX)-minX) * scaleWidth, 0, (max(poly.coordsX)-minX) * scaleWidth, self.height) | |
| poly.attributes['transform'] = ('translate(%s,%s)' % (-minX*scaleWidth, (maxY*scaleHeight)) + ' scale(%s,%s)'%((scaleWidth), -(scaleHeight))) | |
| i = i + 1 | |
| class MyApp(App): | |
| def __init__(self, *args): | |
| super(MyApp, self).__init__(*args) | |
| def main(self): | |
| self.joint_name = joint_name | |
| # Get the joint limits to plot taking them into account | |
| free_joints = get_joint_limits() | |
| print "free joints lookes like: " + str(free_joints) | |
| self.joint_min = free_joints[self.joint_name]['min_position'] | |
| self.joint_max = free_joints[self.joint_name]['max_position'] | |
| self.wid = gui.Widget(1200, 1200, False, 10) | |
| self.first_msg = None | |
| self.sub = rospy.Subscriber('/joint_states', JointState, self.js_cb, queue_size=1) | |
| while self.first_msg is None and not rospy.is_shutdown(): | |
| rospy.sleep(0.1) | |
| # Dropdown of joints: joint_names | |
| self.joint_names_dd = gui.DropDown(-1, -1) | |
| choose_ddi = gui.DropDownItem(-1, -1, "Choose joint...") | |
| self.joint_names_dd.append('0', choose_ddi) | |
| for idx, joint_name in enumerate(self.first_msg.name): | |
| jn_ddi = gui.DropDownItem(-1, -1, joint_name) | |
| self.joint_names_dd.append(str(idx), jn_ddi) | |
| self.joint_names_dd.select_by_key('0') | |
| self.joint_names_dd.set_on_change_listener(self, 'on_change_joint_name') | |
| # Dropdown of fields: position, velocity, effort | |
| self.fields_dd = gui.DropDown(-1, -1) | |
| position_ddi = gui.DropDownItem(-1, -1, "position") | |
| self.fields_dd.append('0', position_ddi) | |
| velocity_ddi = gui.DropDownItem(-1, -1, "velocity") | |
| self.fields_dd.append('1', velocity_ddi) | |
| effort_ddi = gui.DropDownItem(-1, -1, "effort") | |
| self.fields_dd.append('2', effort_ddi) | |
| self.fields_dd.select_by_key('0') | |
| self.fields_dd.set_on_change_listener(self, 'on_change_field') | |
| # Button of Add to plot | |
| self.add_to_plot_button = gui.Button(-1, -1, "Add to plot") | |
| self.add_to_plot_button.set_on_click_listener(self, 'on_add_to_plot_button') | |
| # Dropdown of remove from plot | |
| # Max time to show in plot | |
| default_max_time = 5.0 # 5s | |
| default_max_points = int(default_max_time * 50) # 50Hz joint states | |
| self.hor_widget2 = gui.Widget(600, 40, True, 10) | |
| self.label_len = gui.Label(200, 30, "Max time length") | |
| self.slider_max_len = gui.Slider(200, 30, defaultValue=default_max_points, min=1, max=1000, step=1) | |
| self.slider_max_len.set_on_change_listener(self, 'on_change_max_len_slider') | |
| self.label_max_qtty = gui.Label(50, 30, str(default_max_time)) | |
| self.hor_widget2.append('label', self.label_len) | |
| self.hor_widget2.append('slider', self.slider_max_len) | |
| self.hor_widget2.append('qtty', self.label_max_qtty) | |
| # Append the horizontal slider stuff | |
| self.wid.append('max_slider', self.hor_widget2) | |
| # Save here plots | |
| self.plots = [] | |
| # Create the plot | |
| self.svgplot = SvgPlot(900, 900) | |
| self.plotData1 = gui.SvgPolyline(default_max_points) | |
| self.plotData1.set_stroke(2.0, 'rgba(255,0,0,0.8)') | |
| self.svgplot.append_poly(self.plotData1) | |
| self.wid.append('plot', self.svgplot) | |
| # returning the root widget | |
| return self.wid | |
| def on_change_joint_name(self, value): | |
| pass | |
| def on_change_field(self, value): | |
| pass | |
| def on_add_to_plot_button(self): | |
| pass | |
| def on_change_max_len_slider(self, value): | |
| for plot in self.plots: | |
| # TODO: do stuff for all the plots | |
| continue | |
| num_points = int(value / 50.0) | |
| self.label_max_qtty.set_text(str(value)) | |
| self.svgplot = SvgPlot(900, 900) | |
| self.plotData1 = gui.SvgPolyline(num_points) | |
| self.plotData1.set_stroke(2.0, 'rgba(255,0,0,0.8)') | |
| self.svgplot.append_poly(self.plotData1) | |
| self.wid.append('plot', self.svgplot) | |
| def js_cb(self, msg): | |
| if self.first_msg is None: | |
| self.first_msg = msg | |
| return | |
| x = msg.header.stamp.secs + msg.header.stamp.nsecs / 1000000000.0 | |
| idx_wl = msg.name.index(self.joint_name) | |
| y = msg.position[idx_wl] | |
| print x, y | |
| # TODO: for every plot added, do the stuff, only render in the end | |
| for plot in self.plots: | |
| continue | |
| self.plotData1.add_coord(x, y) | |
| self.svgplot.render(fixed_minY=self.joint_min, fixed_maxY=self.joint_max) | |
| if __name__ == "__main__": | |
| rospy.init_node('asdfasdfasdfa') | |
| start(MyApp) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment