Merge NaviGator into monorepo

.gitmodules

@@ -1,3 +1,9 @@
+[submodule "gnc/lqRRT"]
+	path = NaviGator/gnc/navigator_path_planner/lqRRT
+	url = https://github.com/uf-mil/lqRRT
+[submodule "simulation/vmrc"]
+	path = NaviGator/simulation/vmrc
+	url = https://github.com/uf-mil/vmrc
 [submodule "txros"]
 	path = mil_common/txros
 	url = https://github.com/txros/txros.git

NaviGator/Jenkinsfile

@@ -0,0 +1,50 @@
+dockerNode(image: 'uf-mil:navigator') {
+	stage("Checkout") {
+		checkout scm
+		sh '''
+			source ~/.mil/milrc > /dev/null 2>&1
+			git submodule update --init --recursive
+			ln -s $PWD $CATKIN_DIR/src/NaviGator
+			git clone --recursive https://github.com/uf-mil/mil_common $CATKIN_DIR/src/mil_common
+		'''
+	}
+	stage("Build") {
+		sh '''
+			source ~/.mil/milrc > /dev/null 2>&1
+			catkin_make -C $CATKIN_DIR -B
+		'''
+	}
+	stage("Test") {
+		sh '''#!/bin/bash -i
+			source ~/.mil/milrc > /dev/null 2>&1
+			source $CATKIN_DIR/devel/setup.bash > /dev/null 2>&1
+			$(navtest)
+			catkin_test_results $CATKIN_DIR/build/test_results --verbose
+		'''
+	}
+	stage("Format") {
+		sh '''#!/bin/bash -i
+			source ~/.mil/milrc > /dev/null 2>&1
+			source $CATKIN_DIR/devel/setup.bash > /dev/null 2>&1
+			OUT=$(navfmt)
+			if [ $? -ne 0 ]; then
+				echo $OUT
+				echo "The preceding Python following files are not formatted correctly"
+				exit 1
+			fi
+		'''
+		sh '''
+			source /opt/ros/kinetic/setup.bash > /dev/null 2>&1
+			wget -O ~/.clang-format https://raw.githubusercontent.com/uf-mil/installer/master/.clang-format
+			for FILE in $(find -path ./deprecated -prune -o -path ./satellite -prune -o -path ./simulation/vmrc -prune -o -regex ".*/.*\\.\\(c\\|cc\\|cpp\\|h\\|hpp\\)$"); do
+				if [ ! -z "$(clang-format-3.8 -style=file -output-replacements-xml $FILE | grep '<replacement ')" ]; then
+					FILES+=( "$FILE" )
+				fi
+			done
+			if (( ${#FILES[@]} > 0 )); then
+				echo "The following C++ files are not formatted correctly: ${FILES[@]}"
+				exit 1
+			fi
+		'''
+	}
+}

NaviGator/deprecated/README.md

@@ -0,0 +1,8 @@
+# NaviGator deprecated code
+This directory contains libraries, scripts, and executables which have been abandoned, replaced, or no longer used.
+These files are not compiled or tested, but remain temporarily in the repository for reference or future use.
+
+Note: if code has been moved to mil_common, do not put it in the deprecated folder.
+
+If you add files to the deprecated folder, please preserve the directory structure. For example,
+perception/navigator_vision/2016_old_node.py should be moved to deprecated/perception/navigator_vision/2016_old_node.py

NaviGator/deprecated/gnc/navigator_msg_multiplexer/nodes/wrench_arbiter.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python
+from geometry_msgs.msg import WrenchStamped
+from navigator_msgs.srv import WrenchSelect
+import rospy
+from std_msgs.msg import Bool, String
+
+
+rospy.init_node("wrench_arbiter")
+
+
+class WrenchArbiter(object):
+
+    def __init__(self):
+
+        # Set the four receiving variables
+        self.rc_wrench = None
+	self.rc_emergency = None
+        self.keyboard_wrench = None
+        self.autonomous_wrench = None
+
+        # This is what is used to determine which input should be output
+        self.control = None
+        self.control_inputs = ["rc", "emergency", "keyboard", "autonomous"]
+
+        # ROS stuff - Wrench changing service, final output wrench, and the three input sources
+        rospy.Service("/change_wrench", WrenchSelect, self.change_wrench)
+        self.wrench_pub = rospy.Publisher("/wrench/cmd", WrenchStamped, queue_size=1)
+        self.control_pub = rospy.Publisher("/wrench/current", String, queue_size=1)
+        self.learn = rospy.Publisher("learn", Bool, queue_size=1)
+
+        # Subscribers to listen for wrenches
+        rospy.Subscriber("/wrench/rc", WrenchStamped,
+                         lambda msg: self.republish(msg, control_t="rc", learn=False))
+        rospy.Subscriber("/wrench/emergency", WrenchStamped,
+                         lambda msg: self.republish(msg, control_t="emergency", learn=False))
+        rospy.Subscriber("/wrench/keyboard", WrenchStamped,
+                         lambda msg: self.republish(msg, control_t="keyboard", learn=False))
+        rospy.Subscriber("/wrench/autonomous", WrenchStamped,
+                         lambda msg: self.republish(msg, control_t="autonomous", learn=True))
+
+    def republish(self, msg, control_t, learn):
+        '''
+        Republishes message if it's the correct control type. `learn` will publish to the learn topic
+        to start or stop concurrent learning.
+        '''
+
+        # If there's no control selected, send zeros
+        if self.control is None:
+            msg.wrench.force.x = 0
+            msg.wrench.force.y = 0
+            msg.wrench.torque.z = 0
+            learn = False
+
+        elif control_t != self.control:
+            return
+
+        self.learn.publish(learn)
+        msg.header.frame_id = "/base_link"
+        self.wrench_pub.publish(msg)
+        self.control_pub.publish(self.control)
+
+    def change_wrench(self, req):
+        '''
+        This sets the wrench output to the correct source by setting the "control" variable
+        to the right source.
+        '''
+        rospy.loginfo("Server received request for wrench control change - " + req.str)
+        self.control = req.str if req.str in self.control_inputs else self.control
+
+        self.control_pub.publish(self.control)
+        # Returns the selected control input
+        return self.control
+
+
+if __name__ == "__main__":
+    arb = WrenchArbiter()
+    rospy.spin()

NaviGator/deprecated/mission_control/navigator_launch/launch/bag.launch

@@ -0,0 +1,36 @@
+<launch>
+    <!-- For lidar stuff -->
+    <arg name="oa" default="true" />
+
+    <!-- Stereo right, stereo left, right right cameras -->
+    <arg name="im_proc_sr" default="false" />
+    <arg name="im_proc_sl" default="false" />
+    <arg name="im_proc_rr" default="false" />
+
+    <param name="use_sim_time" value="true"/>
+
+    <include file="$(find navigator_launch)/launch/gnc/tf.launch"/>
+
+    <node name="coordinate_converter" pkg="navigator_tools" type="coordinate_conversion_server.py"/>
+    <node name="bounds_server" pkg="navigator_tools" type="bounds.py"/>
+
+    <rosparam param="bounds">
+    <!-- This is for Lake Wauberg -->
+    enforce: True
+    lla: [[29.535011, -82.303323],
+        [29.534647, -82.304280],
+        [29.533440, -82.303577],
+        [29.533803, -82.302639]]
+    </rosparam>
+
+    <node name="front_right_proc" pkg="image_proc" type="image_proc" ns="/camera/front/right" if="$(arg im_proc_sr)"/>
+    <node name="front_left_proc" pkg="image_proc" type="image_proc" ns="/camera/front/left" if="$(arg im_proc_sl)"/>
+    <node name="starboard_proc" pkg="image_proc" type="image_proc" ns="/camera/starboard" if="$(arg im_proc_rr)"/>
+
+    <node name="action_server" pkg="navigator_tools" type="fake_action_server.py" output="screen"/>
+
+    <group if="$(arg oa)">
+        <include file="$(find navigator_launch)/launch/gnc/object_classification.launch" />
+        <node name="ogrid_arbiter" pkg="navigator_msg_multiplexer" type="ogrid_arbiter.py" output="screen" respawn="true"/>
+    </group>
+</launch>

NaviGator/deprecated/mission_control/navigator_launch/launch/subsystems/shooter.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="navigator_shooter" type="ShooterControl.py" name="shooter_controller">
+    <rosparam file="$(find navigator_shooter)/launch/config.yaml" />
+  </node>
+</launch>
+

NaviGator/deprecated/mission_control/navigator_missions/nav_missions/coral_survey_backup.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+import txros
+import numpy as np
+from mil_misc_tools.text_effects import fprint
+
+@txros.util.cancellableInlineCallbacks
+def main(navigator, **kwargs):
+    fprint("STARTING CORAL SURVEY, GETTING A TOWER BOUY AS A TEST", msg_color="green")
+    yield navigator.database_query("tower")
+    yield navigator.nh.sleep(5)
+    fprint("COMPLETED CORAL SURVEY", msg_color="green")

NaviGator/deprecated/mission_control/navigator_missions/nav_missions/empty_start.py

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+import txros
+import numpy as np
+from mil_misc_tools.text_effects import fprint
+
+@txros.util.cancellableInlineCallbacks
+def main(navigator, **kwargs):
+    fprint("STARTING EMPTY TEST", msg_color="green")
+    yield navigator.nh.sleep(5)
+    fprint("COMPLETED EMPTY TEST", msg_color="green")

NaviGator/deprecated/mission_control/navigator_missions/nav_missions/find_the_break.py

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+import txros
+import numpy as np
+from mil_misc_tools.text_effects import fprint
+
+
+@txros.util.cancellableInlineCallbacks
+def main(navigator, **kwargs):
+    fprint("STARTING FIND THE BREAK, GETTING A BOUY AS A TEST", msg_color="green")
+    # Get all objects within a certain radius of type unknown or buoy
+    # Get all combinations and find the ones with the correct distance
+    fprint("COMPLETED FIND THE BREAK", msg_color="green")

NaviGator/deprecated/mission_control/navigator_missions/nav_missions/lat_long_points.py

@@ -0,0 +1,19 @@
+#!/usr/bin/env python
+import txros
+
+
+@txros.util.cancellableInlineCallbacks
+def main(navigator):
+    while True:
+        target_raw = raw_input("Lat, Long: ")
+        if target_raw == 'q':
+            break
+
+        target = map(float, target_raw.split(','))
+        waypoint = yield navigator.move.to_lat_long(*target)
+
+        go = raw_input("Move {} meters? (y/n) ".format(waypoint.distance))
+        if go == 'y':
+            yield waypoint.go()
+        elif go == 'q':
+            break

NaviGator/deprecated/mission_control/navigator_missions/nav_missions/scan_the_code.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+"""Scan The Code Mission Script."""
+import txros
+from navigator_scan_the_code import ScanTheCodeMission
+from mil_misc_tools.text_effects import fprint
+from navigator import Navigator
+
+___author___ = "Tess Bianchi"
+
+
+def _get_color(c):
+    if c == 'r':
+        return "RED"
+    if c == 'b':
+        return 'BLUE'
+    if c == 'y':
+        return 'YELLOW'
+    if c == 'g':
+        return 'GREEN'
+
+
+class ScanTheCode(Navigator):
+    @txros.util.cancellableInlineCallbacks
+    def run(self, parameters):
+        """Main Script of Scan The Code."""
+        # attempts = kwargs["attempts"]
+        # fprint("ATTEMPTS {}".format(attempts), msg_color="green")
+        self.change_wrench("autonomous")
+        fprint("Moving to stc", msg_color='green')
+        mission = ScanTheCodeMission(self)
+        yield mission.init_(self.tf_listener)
+        pose, look_at = yield mission.initial_position()
+        yield self.move.set_position(pose).look_at(look_at).go()
+        yield self.nh.sleep(1)
+        fprint("Finished getting the initial position", msg_color='green')
+
+        # circle = self.move.circle_point(look_at).go()
+        # circle.addErrback(lambda x: x)
+        # yield mission.correct_pose()
+        # circle.cancel()
+
+        mission.correct_pose(pose)
+        circle = self.move.d_circle_point(look_at, radius=8, granularity=30, direction='cw')
+        # print list(circle)
+        for p in list(circle)[::-1]:
+            if mission.stc_correct:
+                break
+            yield p.go()
+            # yield self.nh.sleep(2)
+
+        fprint("Finished getting the correct stc face", msg_color='green')
+
+        defer = mission.find_colors()
+
+        # TRY WITHOUT CIRCLES
+        colors = None
+        try:
+            colors = yield txros.util.wrap_timeout(defer, 15)
+        except txros.util.TimeoutError:
+            pass
+
+        # TRY WITH CIRCLEs
+        # circle = self.move.d_circle_point(look_at, radius=8, granularity=4, direction='cw')
+        # colors = None
+        # for i in circle:
+        #     try:
+        #         colors = yield txros.util.wrap_timeout(defer, 15)
+        #         break
+        #     except txros.util.TimeoutError:
+        #         yield i.go()
+        #         fprint("go", msg_color="red")
+
+        if colors is None:
+            colors = "r", "g", "b"
+        c1, c2, c3 = colors
+        print colors
+        yield self.mission_params["scan_the_code_color1"].set(_get_color(c1))
+        yield self.mission_params["scan_the_code_color2"].set(_get_color(c2))
+        yield self.mission_params["scan_the_code_color3"].set(_get_color(c3))
+
+    @txros.util.cancellableInlineCallbacks
+    def cleanup(self):
+        """Safe exit of the Scan The Code mission."""
+        yield self.mission_params["scan_the_code_color1"].set("RED")
+        yield self.mission_params["scan_the_code_color2"].set("GREEN")
+        yield self.mission_params["scan_the_code_color3"].set("BLUE")