Final fix

.gitignore

@@ -1 +1,5 @@
+<<<<<<< HEAD
+=======
+*.gch
+>>>>>>> serial_fix
 *.pyc

CMakeLists.txt

@@ -15,10 +15,12 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
+<<<<<<< HEAD
 add_executable(Teleop src/Teleop/Teleop.cpp)
 add_executable(periodic_snapshotter src/periodic_snapshotter)
+=======
+>>>>>>> serial_fix
 add_executable(tf_to_odom src/publish_tf_as_odom)
 
-target_link_libraries(Teleop ${catkin_LIBRARIES})
 target_link_libraries(tf_to_odom ${catkin_LIBRARIES})
 target_link_libraries(periodic_snapshotter ${catkin_LIBRARIES})

launch/bringup_minimal.launch

@@ -1,9 +1,9 @@
 <launch>
   <arg name='port' default='/dev/ttyUSB0'/>
 
-  <include file="$(find crawler)/launch/teleop.launch"/>
   <include file="$(find crawler)/launch/mainstate.launch"/>
 
+  <node pkg="crawler" type="ConvertToAckermann.py" name="twist_to_ackermann"/>
   <node pkg="crawler" type="HindBrain.py" name="hind_brain" output="screen">
     <param name="port" value="$(arg port)" />
   </node>

launch/mainstate.launch

@@ -1,7 +1,7 @@
 <launch>
 
 <rosparam command="load" file="$(find crawler)/config/crawler_behaviors.yaml" />
-<node pkg="state_controller" type="MainState" name="mainstate" output="screen"/>
+<include file="$(find state_controller)/launch/mainstate.launch" />
 <node name="convert_to_ackermann" pkg="crawler" type="ConvertToAckermann.py"/>
 
 </launch>

scripts/HindBrain.py

@@ -4,24 +4,35 @@
 import rospy as rp
 import sys
 from ackermann_msgs.msg import AckermannDrive
-<<<<<<< Updated upstream
-=======
 from geometry_msgs.msg import Pose
 from std_msgs.msg import Bool
 from helper_functions import *
->>>>>>> Stashed changes
+from geometry_msgs.msg import Pose
+from std_msgs.msg import Bool
+from helper_functions import *
 
-# Hello
 class UnoNode():
 
     def __init__(self):
+        """initializes class
+
+        Creates ros constructs, msg/cmd attribute ranges, etc.
+        """
 
         rp.init_node('hind_brain')
         self.ack_sub = rp.Subscriber('/cmd_vel', AckermannDrive, self.ackermann_cb)
+        self.estop_sub = rp.Subscriber('/softestop', Bool, self.estop_cb)
+        self.pose_sub = rp.Subscriber('/cmd_hitch', Pose, self.pose_cb)
         self.rate = rp.Rate(5)
         self.prev_command = ''
         self.name = 'hb'
 
+        self.vel_range = (-1,0,1)
+        self.steer_range = (-45,0,45)
+        self.z_range = (-0.3,0,0.3)
+        self.pitch_range = (-45,0,45)
+        self.msg_range = (0,50,100)
+
         # Setup serial port
         try:
             port = rp.get_param('/hind_brain/port')
@@ -31,10 +42,25 @@ def __init__(self):
             rp.signal_shutdown(e)
 
     def ackermann_cb(self, msg):
+        """callback function for ackermann messages
+
+        Converts ackermann msg into a serial msg, sends if msg is different than
+        last msg sent
+
+        :param[in] msg: ackermann msg provided by ROS subscriber
+        :param[out] self.prev_command: Updates previous command attribute
+        """
+
+        speed_cmd = cmd2msg(msg.speed, self.vel_range, self.msg_range)
+        steer_cmd = cmd2msg(msg.steering_angle, self.steer_range, self.msg_range)
+        command = "!a:" + str(int(speed_cmd)) + ":" + str(int(steer_cmd)) + ":"
+        if (command != self.prev_command):
+            self.send(command)
+            self.prev_command = command
+
+    def estop_cb(self, msg):
+        """callback function for estop messages
 
-<<<<<<< Updated upstream
-        command = "[" + str(msg.speed) + "|" + str(msg.steering_angle) + "]"
-=======
         Converts estop msg into serial msg, sends if msg is different than
         last msg sent
 
@@ -58,36 +84,40 @@ def pose_cb(self, msg):
         z_cmd = cmd2msg(msg.position.z, self.z_range, self.msg_range)
         pitch_cmd = cmd2msg(msg.orientation.y, self.pitch_range, self.msg_range)
         command = "!b:" + str(int(z_cmd)) + ":" + str(int(pitch_cmd)) + ":\n"
->>>>>>> Stashed changes
         if (command != self.prev_command):
-            print("Trasmitted: " + command)
             self.send(command)
             self.prev_command = command
 
     def send(self, msg):
+        """sends msg over serial
+
+        Tries to write msg to serial, assumes lost connectivity if failed
 
+        :param[in] msg: string to send over serial
+        :return: failure boolean
+        """
         try:
             self.uno_port.write(msg.encode('utf-8'))
-<<<<<<< Updated upstream
-=======
             rp.loginfo("%s - Sent message: %s", self.name, msg)
             return True
->>>>>>> Stashed changes
         except Exception as e:
             rp.logerr("%s - lost connectivity with rover", self.name)
             rp.logerr(e)
-<<<<<<< Updated upstream
-=======
             return False
->>>>>>> Stashed changes
+            rp.loginfo("%s - Sent message: %s", self.name, msg)
+            return True
+        except Exception as e:
+            rp.logerr("%s - lost connectivity with rover", self.name)
+            rp.logerr(e)
+            return False
 
     def run(self):
+        """runs node mainloop
+
+        sends watchdog msg at rate attr. frequency if ros is ok
+        """
         while not rp.is_shutdown():
-<<<<<<< Updated upstream
-            self.send('.') # Satisfy watchdog
-=======
             #self.send('!w') # Satisfy watchdog
->>>>>>> Stashed changes
             self.rate.sleep()
 
 if __name__ == '__main__':

scripts/helper_functions.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+
+def mapPrecise(x, inMin, inMax, outMin, outMax):
+  # Emulates Arduino map() function, but uses floats for precision
+  return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
+
+def cmd2msg(c_in, c_range, m_range):
+    # Given command, command range, and msg range, returns corresponding msg
+    m_out = 0
+
+    # Unpack range values for clarity
+    c_low = c_range[0]
+    c_mid = c_range[1]
+    c_hi = c_range[2]
+    m_low = m_range[0]
+    m_mid = m_range[1]
+    m_hi = m_range[2]
+
+    # Convert from input command to output message
+    if c_low < c_hi:  # Ensure correct direction of comparison - is c_high < c_low?
+        if c_in < c_mid: m_out = mapPrecise(c_in, c_low, c_mid, m_low, m_mid)
+        elif c_in > c_mid: m_out = mapPrecise(c_in, c_mid, c_hi, m_mid, m_hi)
+        else: m_out = m_mid
+    elif c_low > c_hi:
+        if c_in > c_mid: m_out = mapPrecise(c_in, c_low, c_mid, m_low, m_mid)
+        elif c_in < c_mid: m_out = mapPrecise(c_in, c_mid, c_hi, m_mid, m_hi)
+        else: m_out = m_mid
+
+    return m_out
+
+def msg2cmd(m_in, m_range, c_range):
+    pass