stateSpaceRobotics · JoshuaBRussell · Sep 29, 2016 · Oct 9, 2016 · Oct 11, 2016 · Oct 11, 2016
diff --git a/.travis.yml b/.travis.yml
@@ -89,7 +89,7 @@ install:
   - cd ~/catkin_ws/src
   - ln -s $CI_SOURCE_PATH .
 
-# Install all dependencies, using wstool and rosdep.
+# Install all dependencies, using wstool, rosdep, and pip.
 # wstool looks for a ROSINSTALL_FILE defined in the environment variables.
 before_script:
   # source dependencies: install using wstool.
@@ -100,6 +100,7 @@ before_script:
   # package depdencies: install using rosdep.
   - cd ~/catkin_ws
   - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
+  - sudo pip install pyusb --pre
 
 # Compile and test (fail if any step fails). If the CATKIN_OPTIONS file exists,
 # use it as an argument to catkin_make, for example to blacklist certain

diff --git a/cr17/CMakeLists.txt b/cr17/CMakeLists.txt
@@ -190,10 +190,15 @@ include_directories(
 # endif()
 
 ## Add folders to be run by python nosetests
-#catkin_add_nosetests(test)
+catkin_add_nosetests(test)
 if(CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
-  add_rostest(test/test_navigator.test)
+  foreach(T
+          test/test_navigator.test
+          test/test_mbed_interface.test
+         )
+  add_rostest(${T})
+  endforeach()
 endif()
 
 # Build transform inverter

diff --git a/cr17/launch/Demo_Teleop.launch b/cr17/launch/Demo_Teleop.launch
@@ -0,0 +1,7 @@
+<launch>
+
+    <node name="mbed_interface" pkg="cr17" type="mbed_interface.py" respawn="true" launch-prefix="sudo -E "  />
+		<node name="joy" pkg="joy" type="joy_node" respawn="true" />
+		<node name="joystick_controller" pkg="cr17" type="joystick_controller.py" respawn="true"  />
+
+</launch>
diff --git a/cr17/launch/control_station.launch b/cr17/launch/control_station.launch
@@ -2,4 +2,12 @@
     <rosparam command="load" file="$(find cr17)/param/control_station.yaml" />
     <node name="joystick" pkg="joy" type="joy_node"/>
     <node pkg="cr17" type="control_station.py" output="screen" name="control_station"/>
+
+    <node pkg="cr17" type="usbcam_receiver.py" name="usbcam_receiver"/>
+
+    <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
+        <remap from="image" to="/usb_cam/image_raw"/>
+        <param name="autosize" value="true" />
+    </node>
+
 </launch>
diff --git a/cr17/launch/mbed_interface_launch.launch b/cr17/launch/mbed_interface_launch.launch
@@ -0,0 +1,5 @@
+<launch>
+
+<node name="mbed_interface" pkg="cr17" type="mbed_interface.py" respawn="true" launch-prefix="sudo -E PYTHONPATH=$(optenv PYTHONPATH) "  />
+
+</launch>
diff --git a/cr17/launch/robot_teleop.launch b/cr17/launch/robot_teleop.launch
@@ -1,13 +1,12 @@
 <?xml version="1.0" encoding="utf-8"?>
 <launch>
 
-    <node pkg="turtle_teleop" type="turtle_teleop_joy" name="teleop">
-        <param name="left_trigger" value="4" type="int"/>
-        <param name="right_trigger" value="5" type="int"/>
-        <param name="left_y_stick" value="1" type="int"/>
-        <param name="right_y_stick" value="3" type="int"/>
-    </node>
+   <include file="$(find cr17)/launch/mbed_interface_launch.launch" />
+
+   <include file="$(find cr17)/launch/usb_cam.launch"/>
+   <node pkg="cr17" type="usbcam_sender.py" name="usbcam_sender" />
 
     <node pkg="cr17" type="joy_repub.py" name="joy_repub" />
+    <node pkg="cr17" type="joystick_controller.py" name="joystick_controller" />
 
 </launch>
diff --git a/cr17/launch/robot_teleop_logitech.launch b/cr17/launch/robot_teleop_logitech.launch
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+    <rosparam command="load" file="$(find cr17)/param/logitech.yaml" />
+    <include file="$(find cr17)/launch/robot_teleop.launch" />
+</launch>
diff --git a/cr17/launch/teleop_tank_logitech.launch b/cr17/launch/teleop_tank_logitech.launch
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+    <rosparam command="load" file="$(find cr17)/param/logitech_tank.yaml" />
+    <include file="$(find cr17)/launch/mbed_interface_launch.launch" />
+
+    <node pkg="cr17" type="joy_repub.py" name="joy_repub" />
+    <node pkg="cr17" type="joystick_controller_tank.py" name="joystick_controller_tank" />
+</launch>
diff --git a/cr17/launch/usb_cam.launch b/cr17/launch/usb_cam.launch
@@ -0,0 +1,15 @@
+<launch>
+  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
+    <param name="video_device" value="/dev/video0" />
+    <param name="image_width" value="640" />
+    <param name="image_height" value="480" />
+    <param name="pixel_format" value="yuyv" />
+    <param name="camera_frame_id" value="usb_cam" />
+    <param name="io_method" value="mmap"/>
+  </node>
+  <node name="usbcam_sender" pkg="cr17" type="usbcam_sender.py" />
+  <!--<node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
+    <remap from="image" to="/usb_cam/image_raw"/>
+    <param name="autosize" value="true" />
+  </node>-->
+</launch>
diff --git a/cr17/nodes/cr17/joystick_controller.py b/cr17/nodes/cr17/joystick_controller.py
@@ -4,6 +4,8 @@
 from sensor_msgs.msg import Joy
 from std_msgs.msg import Int16, String
 from cr17.srv import autonomousActive
+from cr17.msg import scoopControl
+
 
 '''
 This module is used to convert joystick messages into appropriate affector commands.
@@ -12,13 +14,15 @@
 ###################################
 # Constants
 CONTROLLER_BUTTONS = {"A": 0, "B":1, "X": 2, "Y": 3, "R1": 5, "L1": 4, "BACK": 6, "START": 7} # TODO: FINISH THIS, USE BELOW
-CONTROLLER_AXES = {"LSTICKV": 1, "LSTICKH": 0}
+CONTROLLER_AXES = {"LSTICKV": 1, "LSTICKH": 0, "RTRIGGER":5, "LTRIGGER":2}
 # TODO: make these ROS parameters
-MAX_MAG = 7
+MAX_MAG = 1
 SLOP_THRESH = 0.15
+ARM_MOVE_MAG = 10
+HAND_MOVE_MAG = 12
 # Axes to use for drive twists
-JOY_LINEAR_AXIS = CONTROLLER_AXES["LSTICKV"]
-JOY_ANGULAR_AXIS = CONTROLLER_AXES["LSTICKH"]
+JOY_LINEAR_AXIS = rospy.get_param("joy/linear", CONTROLLER_AXES["LSTICKV"])
+JOY_ANGULAR_AXIS = rospy.get_param("joy/angular", CONTROLLER_AXES["LSTICKH"])
 
 ARM_DOWN_AXIS = rospy.get_param("joy/arm_down", 2)
 ARM_UP_AXIS = rospy.get_param("joy/arm_up", 5)
@@ -45,13 +49,16 @@ def __init__(self):
         '''
         rospy.init_node("joystick_controller")
 
-        self.joy_received = False  
+        self.joy_received = False
+        self.arm_state = scoopControl()
+        self.arm_state.armAngle = 50
+        self.arm_state.scoopAngle = 50
 
         global DRIVE_SPEED
         try:
             # Constants for drive speeds
-            DRIVE_SPEED = int(rospy.get_param("drive_settings/drive_speed"))
-        except:
+            DRIVE_SPEED = int(rospy.get_param("drive_settings/drive_speed", 1))
+        except: 
             rospy.logerr("Failed to load motor parameters.")  
 
         self.controller_state = Joy()
@@ -66,18 +73,24 @@ def __init__(self):
         # Load topic names
         self.joystick_topic       = rospy.get_param("topics/joystick", "joy")
         drive_topic               = rospy.get_param("topics/drive_cmds", "cmd_vel")
+        self.arm_state_topic      = rospy.get_param('topics/scoop_state_cmds', "cmd_scoop")
+        self.arm_state_in         = rospy.get_param("topics/scoop_state_current", "scoop_out")
+
         # Setup publishers
         self.drive_pub = rospy.Publisher(drive_topic, Twist, queue_size = 10)
+        self.scoop_pub = rospy.Publisher(self.arm_state_topic, scoopControl, queue_size=10)
+
         # Setup subscribers
         rospy.Subscriber(self.joystick_topic, Joy, self.joy_callback)
+        rospy.Subscriber(self.arm_state_in, scoopControl, self.arm_callback)
 
     def start_teleop(self):
         self.teleopEnabled = True
-        try:
-            autonomousScoopService = rospy.ServiceProxy("/autonomousScoop", autonomousActive)
-            autonomousScoopService(False)
-        except rospy.ServiceException, e:
-            rospy.logerr("/autonomousScoop service call failed: %s",e)
+        # try:
+        #     autonomousScoopService = rospy.ServiceProxy("/autonomousScoop", autonomousActive)
+        #     autonomousScoopService(False)
+        # except rospy.ServiceException, e:
+        #     rospy.logerr("/autonomousScoop service call failed: %s",e)
 
         try:
             autonomousNavigatorService = rospy.ServiceProxy("/autonomousNavigator", autonomousActive)
@@ -125,6 +138,11 @@ def joy_callback(self, data):
                     self.start_teleop()
         self.teleopButton_prev = data.buttons[TELEOP_BUTTON]
 
+    def arm_callback(self, data):
+        self.arm_state = data
+
+    def combineCleanArm(self, armDown, armUp):
+        return (-1*(armUp-1))+(armDown-1)
 
     def run(self):
         '''
@@ -138,6 +156,7 @@ def run(self):
         while not rospy.is_shutdown():
             # Grab most recent controller state
             current_state = self.controller_state
+            self.joy_received = False
             ######
             # Build Twist message
             ######
@@ -152,14 +171,14 @@ def run(self):
             if (lin_val <= SLOP_THRESH) and (lin_val >= -SLOP_THRESH):
                 # Within 0 point slop
                 lin_vel = 0
-            else:   
-                lin_vel = (lin_val / mag) * MAX_MAG
+            else:
+                lin_vel = lin_val * MAX_MAG#(lin_val / mag) * MAX_MAG
 
             if (ang_val <= SLOP_THRESH) and (ang_val >= -SLOP_THRESH):
                 # Within 0 point slop
                 ang_vel = 0
             else:   
-                ang_vel = (ang_val / mag) * MAX_MAG
+                ang_vel = ang_val * MAX_MAG #(ang_val / mag) * MAX_MAG
 
             lin_history.pop(0)
             vel_history.pop(0)
@@ -174,7 +193,27 @@ def run(self):
             if self.teleopEnabled:
                 self.drive_pub.publish(twister)
 
-            self.joy_received = False
+            arm_up = current_state.axes[ARM_UP_AXIS]
+            arm_down = current_state.axes[ARM_DOWN_AXIS]
+
+            hand_up = current_state.buttons[HAND_UP_BUTTON]
+            hand_down = current_state.buttons[HAND_DOWN_BUTTON]
+
+            arm_change = self.combineCleanArm(arm_down, arm_up)
+            hand_change = hand_up - hand_down
+
+            try:
+                arm_change = arm_change/abs(arm_change) #this is temporary and just for the video
+            except Exception:
+                arm_change = 0
+
+            desired_scoop_state = scoopControl()
+
+            desired_scoop_state.armAngle = self.arm_state.armAngle + (ARM_MOVE_MAG*arm_change)
+            desired_scoop_state.scoopAngle = self.arm_state.scoopAngle + (HAND_MOVE_MAG*hand_change)
+
+            self.scoop_pub.publish(desired_scoop_state)
+
             rate.sleep()
 
 if __name__ == "__main__":