Commits

iorodeo committed 019cab2

Added inertial trajectory commands to GUI.

  • Participants
  • Parent commits 134302f

Comments (0)

Files changed (2)

software/gui/nodes/sled_control.py

             self.robotControl.disableLogger()
             self.robotControl.disableDynamics()
             self.robotControl.setPwmToDefault()
+            self.robotControl.disableLaserSetpt()
 
             self.statusbar.showMessage('System Enabled - Stopped')
             if self.logFileName is not None:
             self.robotControl.stopActuatorOutscan()
             self.robotControl.disableControllerMode()
             self.robotControl.disableDynamics()
+            self.robotControl.disableLaserSetpt()
             self.progressBar.setVisible(False)
             self.writeStatusMessage('%s stopped'%(self.startupMode,))
 
             damping = run['damping'][0]
             self.robotControl.setDynamicsParams(mass,damping)
             self.startOutscan(values, 'actuator')
+        elif self.startupMode == 'inertial trajectory':
+            # ---------------------------------------------------
+            # Disabled to prevent the curious for breaking shit
+            #self.startOutscan(values, 'actuator')
+            # ----------------------------------------------------
         else:
             raise ValueError, 'unknown startup mode'
 

software/utilities/src/utilities/robot_control.py

                     'set_dynam_params',
                     SetDynamParams,
                     )
+        if self.mode == 'inertial trajectory':
+            rospy.wait_for_service('laser_setpt_enable')
+            self.laserSetptEnableProxy = rospy.ServiceProxy(
+                    'laser_setpt_enable',
+                    NodeEnable,
+                    )
 
         # Setup action clients
         self.setptActionClient = actionlib.SimpleActionClient(
         else:
             return True, ''
 
+    def enableLaserSetpt(self):
+        """
+        Enables the set point source generated by the short range laser distance
+        sensor.
+        """
+        if not self.mode == 'inertial trajectory':
+            raise IOError, 'can only enable laser set point in inertial trajectory mode'
+        resp = self.laserSetptEnableProxy(True)
+        return resp.status, resp.message
+
+    def disableLaserSetpt(self):
+        """
+        Disabled the set point source generated by the short range laser distance 
+        sensor.
+        """
+        if self.mode == 'inertial trajectory':
+            resp = self.laserSetptEnableProxy(True)
+            return resp.status, resp.message
+        else:
+            return True, ''
+
     def setDynamicsParams(self,mass,damping):
         """
         Sets the mass and damping for captive trajectory mode.
         if self.mode == 'captive trajectory':
             self.setPointRelToAbsReset()
             self.enableDynamics()
+        elif self.mode == 'inertial trajectory':
+            self.enableLaserSetpt()
+        else:
+            raise ValueError, 'unknown mode {0}'.format(self.mode)
 
         self.actuatorActionClient.send_goal(
                 goal,