Merge lp:~shadowrobot/sr-visualization/Bug_grasp_controller_crash_fuerte into lp:sr-visualization/fuerte

[Toni Oliver]

Fixed grasp_controller crash when starting the plugin without running controllers.

https://app.asana.com/0/1470949453683/1682425321707

[markpitchless (markpitchless)]

http://reviewboard/r/101/

=== modified file '.bzrignore'
--- .bzrignore	2012-03-23 17:46:16 +0000
+++ .bzrignore	2012-11-02 19:17:25 +0000
@@ -20,3 +20,9 @@
 sr_gui_controller_tuner/.cproject
 sr_gui_controller_tuner/.project
 sr_gui_controller_tuner/.pydevproject
+sr_gui_grasp_controller/.cproject
+sr_gui_grasp_controller/.project
+sr_gui_grasp_controller/.pydevproject
+sr_gui_joint_slider/.cproject
+sr_gui_joint_slider/.project
+sr_gui_joint_slider/.pydevproject

=== modified file 'sr_gui_grasp_controller/src/sr_gui_grasp_controller/grasp_controller.py'
--- sr_gui_grasp_controller/src/sr_gui_grasp_controller/grasp_controller.py	2012-08-09 12:39:54 +0000
+++ sr_gui_grasp_controller/src/sr_gui_grasp_controller/grasp_controller.py	2012-11-02 19:17:25 +0000
@@ -379,6 +379,17 @@
 
     def set_reference_grasp(self):
         self.current_grasp.joints_and_positions = self.sr_lib.read_all_current_positions()
+        
+        if self.current_grasp.joints_and_positions is None:
+            #We try to activate ethercat hand again (detect if any controllers are running, and listen to them to extract the current position)
+            self.sr_lib.activate_etherCAT_hand()
+            #Some time to start receiving data
+            rospy.sleep(0.5)
+            self.current_grasp.joints_and_positions = self.sr_lib.read_all_current_positions()
+        
+            if self.current_grasp.joints_and_positions is None:
+                QMessageBox.warning(self._widget, "Warning", "Could not read current grasp.\nCheck that the hand controllers are running.\nThen click \"Set Reference\"")
+                return
 
         self.grasp_interpoler_1 = GraspInterpoler(self.grasp_from_chooser.grasp, self.current_grasp)
         self.grasp_interpoler_2 = GraspInterpoler(self.current_grasp, self.grasp_to_chooser.grasp)
@@ -387,10 +398,19 @@
 
     def grasp_changed(self):
         self.current_grasp.joints_and_positions = self.sr_lib.read_all_current_positions()
+        
+        if self.current_grasp.joints_and_positions is None:
+            QMessageBox.warning(self._widget, "Warning", "Could not read current grasp.\nCheck that the hand controllers are running.\nThen click \"Set Reference\"")
+            return
+        
         self.grasp_interpoler_1 = GraspInterpoler(self.grasp_from_chooser.grasp, self.current_grasp)
         self.grasp_interpoler_2 = GraspInterpoler(self.current_grasp, self.grasp_to_chooser.grasp)
 
     def interpolate_grasps(self, value):
+        if self.grasp_interpoler_1 is None \
+            or self.grasp_interpoler_2 is None:
+            QMessageBox.warning(self._widget, "Warning", "Could not read current grasp.\nCheck that the hand controllers are running.\nThen click \"Set Reference\"")
+            return
         #from -> current
         if value < 0:
             targets_to_send = self.grasp_interpoler_1.interpolate(100 + value)