Merge branch 'atp-smart-servo-ros1-578' of https://github.com/space-c…

…oncordia-robotics/robotics-prototype into atp-smart-servo-ros1-578

robot/rospackages/src/mcu_control/config/joy_comms_config.yaml

@@ -22,6 +22,6 @@ controller_mappings:
     - ["BUTTON_SQUARE","set_motor_speeds 0 0 0 0 0 -250",10,250]
   - - ["TRIGGER_L2","move_wheels %a %a %a -%a -%a -%a",10,250]
     - ["TRIGGER_R2","move_wheels -%a -%a -%a %a %a %a",10,250]
-    - ["JOY_RIGHT_X","move_wheels %a %a %a %a %a %a",10,250]
+    - ["JOY_RIGHT_X","move_wheels -%a -%a -%a -%a -%a -%a",10,250]
     - ["BUTTON_CROSS","blink_toggle 1",10,250]
     - ["BUTTON_CIRCLE","blink_toggle 0",10,250]

robot/rospackages/src/mcu_control/config/joy_comms_test_config.yaml

@@ -0,0 +1,27 @@
+enable_button: 5
+next_layout_button: "BUTTON_OPTION"
+previous_layout_button: "BUTTON_SHARE"
+controller_type: 1
+#number of commands has to match the number of mappings in controller_mappings and topics in command_topics
+stop_commands: 
+  - "motors_stop"
+  - "motors_estop"
+#number of topics has to match the number of mappings in controller_mappings and commands in stop_commands
+command_topics: 
+  - /arm_command
+  - /rover_command
+#number of mappings has to match the number of topics in command_topics and commands in stop_commands
+controller_mappings:
+  - - ["TRIGGER_R2","set_motor_speeds %a 0 0 0 0 0",10,250]
+    - ["TRIGGER_L2","set_motor_speeds -%a 0 0 0 0 0",10,250]
+    - ["JOY_LEFT_Y","set_motor_speeds 0 %a 0 0 0 0",10,250]
+    - ["JOY_LEFT_X","set_motor_speeds 0 0 %a 0 0 0",10,250]
+    - ["JOY_RIGHT_Y","set_motor_speeds 0 0 0 %a 0 0",10,250]
+    - ["JOY_RIGHT_X","set_motor_speeds 0 0 0 0 %a 0",10,250]
+    - ["BUTTON_CIRCLE","set_motor_speeds 0 0 0 0 0 250",10,250]
+    - ["BUTTON_SQUARE","set_motor_speeds 0 0 0 0 0 -250",10,250]
+  - - ["TRIGGER_L2","move_wheels %a %a %a -%a -%a -%a",10,250]
+    - ["TRIGGER_R2","move_wheels -%a -%a -%a %a %a %a",10,250]
+    - ["JOY_RIGHT_X","move_wheels -%a -%a -%a -%a -%a -%a",10,250]
+    - ["BUTTON_CROSS","blink_toggle 1",10,250]
+    - ["BUTTON_CIRCLE","blink_toggle 0",10,250]

robot/rospackages/src/mcu_control/launch/camera_streams.launch

@@ -0,0 +1,18 @@
+<launch>
+  <node name="video0" pkg="cv_camera" type="cv_camera_node">
+    <param name="device_id" value="1" />
+    <param name="rate" value="5" />
+  </node>
+  <node name="video1" pkg="cv_camera" type="cv_camera_node">
+    <param name="device_id" value="2" />
+    <param name="rate" value="5" />
+  </node>
+  <node name="video2" pkg="cv_camera" type="cv_camera_node">
+    <param name="device_id" value="3" />
+    <param name="rate" value="5" />
+  </node>
+  <node name="video3" pkg="cv_camera" type="cv_camera_node">
+    <param name="device_id" value="4" />
+    <param name="rate" value="5" />
+  </node>
+</launch>

robot/rospackages/src/mcu_control/launch/joy_comms_test.launch

@@ -0,0 +1,13 @@
+<launch>
+
+  <arg name="joy_dev" default="/dev/input/js0"/>
+  <arg name="joy_comms_config_file" default="$(find mcu_control)/config/joy_comms_test_config.yaml"/>
+
+    <node name="joy_comms" pkg="mcu_control" type="joy_comms_control_node" output="screen">
+      <rosparam command="load" file="$(arg joy_comms_config_file)"/>
+    </node>
+
+    <node name="joy_test" pkg="mcu_control" type="JoyTestNode.py" output="screen">
+    </node>
+
+</launch>

robot/rospackages/src/mcu_control/scripts/CommsNode.py

@@ -55,6 +55,9 @@ def get_handler(commandId, selectedDevice):
 
 
 ser = None
+# HACK bcs science teensy is USB
+ser_science = None
+ser_hardware = None
 
 STOP_BYTE = 0x0A
 
@@ -86,6 +89,8 @@ def main():
 
 
 def send_queued_commands():
+    global ser, ser_science
+
     if (len(arm_queue) > 0):
         arm_command = arm_queue.popleft()
         send_command(arm_command[0], arm_command[1], arm_command[2])
@@ -95,8 +100,13 @@ def send_queued_commands():
         send_command(rover_command[0], rover_command[1], rover_command[2])
 
     if (len(science_queue) > 0):
+        # HACK for science on rover
+        if not local_mode:
+            ser = ser_science
         science_command = science_queue.popleft()
         send_command(science_command[0], science_command[1], science_command[2])
+        if not local_mode:
+            ser = ser_hardware
 
 if local_mode:
     # in local mode can only simulate connection to one device
@@ -106,6 +116,12 @@ def send_queued_commands():
 
 def receive_message():
     for device in device_range:
+        if not local_mode:
+            if device == SCIENCE_SELECTED:
+                ser = ser_science
+            else:
+                ser = ser_hardware
+
         gpio.output(SW_PINS, PIN_DESC[device])
         if ser.in_waiting > 0:
 
@@ -144,6 +160,7 @@ def receive_message():
                 print(e)
 
 def send_command(command_name, args, deviceToSendTo):
+    print("port: ", ser.port)
     command = get_command(command_name, deviceToSendTo)
     if command is not None:
         commandID = command[1]
@@ -222,7 +239,9 @@ def get_arg_bytes(command_tuple):
     if local_mode:
         ser = serial.Serial('/dev/ttyACM0', 57600, timeout = 1)
     else:
-        ser = serial.Serial('/dev/ttyTHS2', 57600, timeout = 1)
+        ser_science = serial.Serial('/dev/ttyACM0', 57600, timeout = 1)
+        ser_hardware = serial.Serial('/dev/ttyTHS2', 57600, timeout = 1)
+        ser = ser_hardware
 
     node_name = 'comms_node'
     rospy.init_node(node_name, anonymous=False) # only allow one node of this type

robot/rospackages/src/mcu_control/scripts/JoyTestNode.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python3
+
+import rospy
+from sensor_msgs.msg import Joy
+import time
+
+class PlayStation(Joy):
+    _button_names = {"BUTTON_CROSS": 0,
+                    "BUTTON_CIRCLE": 1,
+                    "BUTTON_TRIANGLE": 2,
+                    "BUTTON_SQUARE": 3,
+                    "BUTTON_L1": 4,
+                    "BUTTON_R1": 5,
+                    "BUTTON_L2": 6,
+                    "BUTTON_R2": 7,
+                    "BUTTON_SHARE": 8,
+                    "BUTTON_OPTION": 9,
+                    "BUTTON_HOME": 10,
+                    "BUTTON_L3": 11,
+                    "BUTTON_R3": 12}
+    _axis_names = {"JOY_LEFT_X": 0,
+                    "JOY_LEFT_Y": 1,
+                    "TRIGGER_L2": 2,
+                    "JOY_RIGHT_X": 3,
+                    "JOY_RIGHT_Y": 4,
+                    "TRIGGER_R2": 5,
+                    "DPAD_X": 6,
+                    "DPAD_Y": 7}
+
+    def __init__(self):
+        Joy.__init__(self)
+        self.buttons = [0 for x in range(len(PlayStation._button_names))]
+        self.axes = [0.0 for x in range(len(PlayStation._axis_names))]
+
+    def get_button(self, button_name):
+        return self.buttons[PlayStation._button_names[button_name]]
+
+    def set_button(self, button_name, value):
+        self.buttons[PlayStation._button_names[button_name]] = value
+
+    def get_axis(self, axis_name):
+        return self.axes[PlayStation._axis_names[axis_name]]
+
+    def set_axis(self, axis_name, value):
+        self.axes[PlayStation._axis_names[axis_name]] = value
+
+    def add_to_axis(self, axis_name, increment):
+        self.axes[PlayStation._axis_names[axis_name]] += increment
+
+
+
+
+if __name__ == "__main__":
+    node_name = 'joy_test_node'
+    rospy.init_node(node_name, anonymous=False) # only allow one node of this type
+
+    joy_topic = '/joy'
+    rospy.loginfo('Beginning to publish to "'+joy_topic+'" topic')
+    joyPub = rospy.Publisher(joy_topic, Joy, queue_size=10)
+
+    j = PlayStation()
+    j.set_button("BUTTON_R1", 1) # enable
+
+    # ping us into wheel mode
+    time.sleep(1)
+    joyPub.publish(j)
+    j.set_button("BUTTON_SHARE", 1)
+    joyPub.publish(j)
+    j.set_button("BUTTON_SHARE", 0)
+    time.sleep(1)
+
+    rate = rospy.Rate(2)
+    increment = 0.125
+
+    # R2 partly pressed to test summation
+    j.set_axis("TRIGGER_R2", 0.5)
+
+    while not rospy.is_shutdown():
+        j.add_to_axis("JOY_RIGHT_X", increment)
+
+        if j.get_axis("JOY_RIGHT_X") >= 1.0 or j.get_axis("JOY_RIGHT_X") <= -1.0:
+            # stay at max a little while
+            for i in range(5):
+                joyPub.publish(j)
+                time.sleep(0.5)
+            increment = -increment
+
+        joyPub.publish(j)
+
+        rate.sleep()
+

robot/rospackages/src/mcu_control/src/joy_comms_control.cpp

@@ -11,6 +11,7 @@
 #include <string>
 #include <vector>
 #include <iostream>
+#include <algorithm>
 #include <boost/thread/thread.hpp>
 
 #include "joy_comms_control.h"
@@ -60,6 +61,8 @@ struct JoyCommsControl::Implement {
     int current_mappings_index = 0;
 
     bool change_layout_button_held = false;
+
+    float motor_max = 250.0;
 };
 
 struct JoyCommsControl::Implement::ButtonMappings {
@@ -315,7 +318,15 @@ void JoyCommsControl::publish_command_with_rate() {
                     newCommandAsString.append(commandAsString.substr(index+2, commandAsString.length()));
                 }else{//the axis is treated as an axes. pass percentage multiplied by range
                     int range = pImplement->axis_ranges[pImplement->current_mappings_index][i];
-                    newCommandAsString.append(std::to_string(pImplement->axes_percentage[i] * range));
+                    float value = pImplement->axes_percentage[i] * range;
+
+                    // handle -
+                    if (newCommandAsString.back() == '-') {
+                        value *= -1;
+                        newCommandAsString.pop_back();
+                    }
+
+                    newCommandAsString.append(std::to_string(value));
                     newCommandAsString.append(commandAsString.substr(index+2, commandAsString.length()));
                 }
 
@@ -325,28 +336,40 @@ void JoyCommsControl::publish_command_with_rate() {
             std_msgs::String command;
             command.data = newCommandAsString;
             commands.push_back(command);
+            // std::cout << "adding command: " << command << " axis index " << i << " axis range " << pImplement->axis_ranges[pImplement->current_mappings_index][i] << "\n";
         }
     }
 
-    if(commands.size() > 0){
-        //special case for arm_command
-        if(pImplement->command_topics[pImplement->current_mappings_index] == "/arm_command"){
-            //check if the command is "set_motor_speeds" which has the special case
-            if(commands[0].data.substr(0,commands[0].data.find(' ')) == "set_motor_speeds"){
+    if(commands.size() > 0) {
+        //special case for arm_command and rover_commmand command
+        if(pImplement->command_topics[pImplement->current_mappings_index] == "/arm_command"
+           || pImplement->command_topics[pImplement->current_mappings_index] == "/rover_command") {
+            //check if the command is "set_motor_speeds" or "rover_command" which have the special case
+            std::string command_name = commands[0].data.substr(0,commands[0].data.find(' '));
+            if(command_name == "set_motor_speeds" || command_name == "move_wheels") {
                 float motors [6];
                 std::fill(motors, motors+6, 0);
 
                 //parse the motor values into seperate motor variables
                 for(int i = 0; i < commands.size(); ++i){
                     std::string commandAsString = commands[i].data;
-                    for(int j =0; j < 6; ++j){
-                        commandAsString = commandAsString.substr(commandAsString.find(' ')+1, commandAsString.length());
-                        motors[j] += std::stof(commandAsString.substr(0, commandAsString.find(' ')));
+                    // Only do this if it is the appropriate command
+                    if (commandAsString.rfind(command_name, 0) == 0) {
+                        for(int j = 0; j < 6; ++j) {
+                            commandAsString = commandAsString.substr(commandAsString.find(' ') + 1, commandAsString.length());
+                            motors[j] += std::stof(commandAsString.substr(0, commandAsString.find(' ')));
+                        }
                     }
                 }
 
+                // clamp value
+                for (int i = 0; i < 6; i++) {
+                    motors[i] = std::min(motors[i], pImplement->motor_max);
+                    motors[i] = std::max(motors[i], -(pImplement->motor_max));
+                }
+
                 //rebuild the command with the values of all motors
-                std::string combinedCommandAsString = "set_motor_speeds";
+                std::string combinedCommandAsString = command_name;
                 for(int i =0; i < 6; ++i){
                     combinedCommandAsString.append(" ").append(std::to_string(motors[i]));
                 }
@@ -368,6 +391,8 @@ void JoyCommsControl::publish_command_with_rate() {
     loop_rate.sleep();
 }
 
+
+
 void JoyCommsControl::Implement::publish_command(std_msgs::String command) {
     comms_pubs[current_mappings_index].publish(command);
     sent_disable_msg = false;

robot/rover/internal_comms/src/CommandCenter.cpp

@@ -76,7 +76,7 @@ namespace internal_comms {
         unsigned long start = millis();
         while (!Serial.available()) {
             unsigned long current = millis() - start;
-            if (current > 50) break;
+            if (current > 75) break;
         }
         return Serial.read();
     }

robot/util/rosRoverStart/rover.launch

@@ -10,4 +10,5 @@
     <!-- <node pkg="odroid_rx" type="rx_publisher.py" name="rx_publisher" output="screen"/> -->
 
     <include file="$(find mcu_control)/launch/joy_comms_manual.launch" />
+    <include file="$(find mcu_control)/launch/camera_streams.launch" />
 </launch>