improved joystick interface for constraints.

* merged the joystick axis mapping into main node
* added a stop button (0, by default)

feature_constraints_standalone/launch/joystick_commands.launch

@@ -4,20 +4,12 @@
     <param name="deadzone" value="0.1"/>
   </node>
 
-  <node name="joy_to_float" pkg="joy_to_twist" type="joy_to_floats.py">
+  <node name="joystick_commander" pkg="feature_constraints_standalone"
+        type="constraint_commander.py">
     <rosparam ns="mapping">
       [0, 1, 3, 4, _, _]
     </rosparam>
-    <remap from="/floats" to="/constraint_vel"/>
-  </node>
-
-  <node name="joystick_commander" pkg="feature_constraints_standalone"
-        type="constraint_commander.py">
-    <remap from="/velocity" to="/constraint_vel"/>
     <remap from="/command" to="/feature_controller/constraint_command"/>
-    <remap from="/position" to="/constraint_pos"/>
+    <remap from="/state" to="/feature_controller/constraint_state"/>
   </node>
-
-  <node name="initial_constraints_publisher" pkg="rostopic" type="rostopic" args="pub /constraint_pos std_msgs/Float64MultiArray '{data: [0.887459, 0.998564, 1.49226, -0.735284, -0.465815, 0.0523682]}'"/>
-
 </launch>

feature_constraints_standalone/scripts/constraint_commander.py

@@ -2,51 +2,61 @@
 
 from math import *
 
-import roslib; roslib.load_manifest('feature_constraints_standalone')
+import roslib;
+roslib.load_manifest('feature_constraints_standalone')
+roslib.load_manifest('joy_to_twist')
+
 import rospy
 
 rospy.init_node('constraint_commander')
 
-from std_msgs.msg import Float64MultiArray
-from constraint_msgs.msg import ConstraintCommand
+from sensor_msgs.msg import Joy
+from constraint_msgs.msg import ConstraintCommand, ConstraintState
 
 eps = rospy.get_param('~eps', 0.02)
 scale = rospy.get_param('~scale', 0.03)
+reset_button = rospy.get_param('~reset_button', 0)
 
+chi = []
 command = ConstraintCommand()
 
+import mapping
+button_mapping = mapping.MultiMapping(default_mapping=[0, 1, 2, 3])
+
 def command_init(values):
   command.pos_lo = [x-eps for x in values]
   command.pos_hi = [x+eps for x in values]
   command.weight = [1.0]*len(values)
 
 
-def callback_change(msg):
+def callback_joy(msg):
   global command
+  global chi
+
+  if msg.buttons[reset_button] == 1:
+    command_init(chi)
 
-  if len(command.weight) != len(msg.data):
-    command_init([0.0]*len(msg.data))
+  values = button_mapping(msg)
 
-  values = msg.data
-  for i in range(len(values)):
+  for i in range(min(len(values), len(chi))):
     command.pos_lo[i] += values[i]*scale
     command.pos_hi[i] += values[i]*scale
 
   pub.publish(command)
 
 
-def callback_set(msg):
+def callback_state(msg):
+  global chi
   global command
+  chi = msg.chi
 
-  command_init(msg.data)
-
-  pub.publish(command)
-
+  if len(command.weight) == 0:
+    command_init(chi)
 
 if __name__ == "__main__":
   # connect and start
-  sub_change = rospy.Subscriber("/velocity", Float64MultiArray, callback_change)
-  sub_set    = rospy.Subscriber("/position", Float64MultiArray, callback_set)
+  sub_change = rospy.Subscriber("/joy", Joy, callback_joy)
+  sub_state    = rospy.Subscriber("/state", ConstraintState, callback_state)
   pub = rospy.Publisher('/command', ConstraintCommand)
   rospy.spin()
 

joy_to_twist/manifest.xml

@@ -12,6 +12,12 @@
   <depend package="rospy"/>
   <depend package="joy"/>
   <depend package="geometry_msgs"/>
+
+  <export>
+    <python path="${prefix}"/>
+  </export>
+
+
 </package>