From f6205b565ad84137170858e1260d72edd4494475 Mon Sep 17 00:00:00 2001
From: GarrettChristian <garrettChristian2@gmail.com>
Date: Wed, 21 Apr 2021 11:47:19 -0400
Subject: [PATCH] added pose support in run file

---
 .../launch/hardware_interface.launch          |  6 ++-
 cart_planning/scripts/local_planner.py        |  1 -
 run.sh                                        | 40 +++++++++++++++++--
 velodyne_setup.sh                             |  8 +++-
 velodyne_setup.sh.old                         | 17 ++++++++
 velodyne_setup_tmp.sh                         | 17 ++++++++
 6 files changed, 81 insertions(+), 8 deletions(-)
 create mode 100755 velodyne_setup.sh.old
 create mode 100755 velodyne_setup_tmp.sh

diff --git a/cart_endpoints/launch/hardware_interface.launch b/cart_endpoints/launch/hardware_interface.launch
index a5e57e55..3aef6bb7 100644
--- a/cart_endpoints/launch/hardware_interface.launch
+++ b/cart_endpoints/launch/hardware_interface.launch
@@ -17,7 +17,11 @@
    </group> 
       
    <group>
-      <node name="pointcloud_to_laserscan_node" pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" args="cloud_in:=/velodyne_points scan:=/pcd_to_scan" output="screen" />
+      <node name="pointcloud_to_laserscan_node" pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" args="cloud_in:=/velodyne_points scan:=/pcd_to_scan" output="screen">
+        <param name="min_height" type="double" value="-1.0" />
+        <param name="max_height" type="double" value="0" />
+        <param name="range_min" type="double" value="0.8" />
+      </node>
    </group>
 
     
diff --git a/cart_planning/scripts/local_planner.py b/cart_planning/scripts/local_planner.py
index d18819c8..269748eb 100755
--- a/cart_planning/scripts/local_planner.py
+++ b/cart_planning/scripts/local_planner.py
@@ -303,7 +303,6 @@ def update(self, state, a, delta):
         # Check if any node wants us to stop
 
         # Slow, normal stop
-        print(self.stop_requests)
         if any([x[0] for x in self.stop_requests.values()]):
             msg.vel = 0
 
diff --git a/run.sh b/run.sh
index f34a4eb8..e7584113 100755
--- a/run.sh
+++ b/run.sh
@@ -1,4 +1,22 @@
 #!/bin/bash
+
+# Check for fake pose flag
+fake_pose_flag=''
+
+print_usage() {
+  printf "Usage: -p activates fake_pose\n"
+}
+
+while getopts 'p' flag; do
+  case "${flag}" in
+    p) fake_pose_flag='true' ;;
+    *) print_usage
+       exit 1 ;;
+  esac
+done
+
+# Start everything 
+
 sudo modprobe -r uvcvideo
 echo "Configuring Velodyne..."
 ./velodyne_setup.sh
@@ -9,11 +27,25 @@ sleep 2
 echo "Launching Navigation Code..."
 gnome-terminal --tab -e 'sh -c "roslaunch cart_control navigation.launch obstacle_detection:=true; exec bash"'
 sleep 5
-echo "Starting pose tracking server..."
-gnome-terminal --tab -e 'sh -c "cd ~; cd Desktop/pose-tracking; npm start; exec bash"'
+# echo "Starting pose tracking server..."
+# gnome-terminal --tab -e 'sh -c "cd ~; cd Desktop/pose-tracking; npm start; exec bash"'
 sleep 5
 echo "Starting local server..."
-gnome-terminal --tab -e 'sh -c "cd ~; cd Desktop/jakart-local-server; HTTPS=true npm start; exec bash"'
+# gnome-terminal --tab -e 'sh -c "cd ~; cd Desktop/jakart-local-server; HTTPS=true npm start; exec bash"'
 echo "Starting UI"
-gnome-terminal --tab -e 'sh -c "cd ~; cd Desktop/jakart-cart-ui; HTTPS=true npm start; exec bash"'
+# gnome-terminal --tab -e 'sh -c "cd ~; cd Desktop/jakart-cart-ui; HTTPS=true npm start; exec bash"'
+
+# Pose tracking
+if [ -z "$fake_pose_flag" ]
+then
+    # launch pose
+    echo "Starting pose tracking"
+    gnome-terminal --tab -e 'roslaunch zed_wrapper zed.launch'
+    sleep 5
+    gnome-terminal --tab -e 'python ../jacart-zed/pose/pose_tracking.py'
 
+else
+    # launch fake pose
+    echo "Starting fake pose tracking"
+    gnome-terminal --tab -e 'python ../jacart-zed/pose/fake_pose_tracking.py'
+fi
diff --git a/velodyne_setup.sh b/velodyne_setup.sh
index f9bc7b2c..7be570ee 100755
--- a/velodyne_setup.sh
+++ b/velodyne_setup.sh
@@ -8,10 +8,14 @@ nmcli radio wifi off
 
 echo "Assigning IP to Port"
 
-sudo ifconfig eno1 192.168.3.100
+sudo ifconfig enp0s31f6 192.168.3.100
 
 echo "Adding static route to LIDAR IP"
 
-sudo route add 192.168.1.201 eno1
+sudo route add 192.168.1.201 enp0s31f6
 
 echo "Try running the velodyne manager now"
+
+sleep 5
+
+nmcli radio wifi on
diff --git a/velodyne_setup.sh.old b/velodyne_setup.sh.old
new file mode 100755
index 00000000..f9bc7b2c
--- /dev/null
+++ b/velodyne_setup.sh.old
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+echo "Running Velodyne Setup Script"
+
+echo "Shutting Down WiFi"
+
+nmcli radio wifi off
+
+echo "Assigning IP to Port"
+
+sudo ifconfig eno1 192.168.3.100
+
+echo "Adding static route to LIDAR IP"
+
+sudo route add 192.168.1.201 eno1
+
+echo "Try running the velodyne manager now"
diff --git a/velodyne_setup_tmp.sh b/velodyne_setup_tmp.sh
new file mode 100755
index 00000000..930ccd2e
--- /dev/null
+++ b/velodyne_setup_tmp.sh
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+echo "Running Velodyne Setup Script"
+
+echo "Shutting Down WiFi"
+
+nmcli radio wifi off
+
+echo "Assigning IP to Port"
+
+sudo ifconfig enp0s31f6 192.168.3.100
+
+echo "Adding static route to LIDAR IP"
+
+sudo route add 192.168.1.201 enp0s31f6
+
+echo "Try running the velodyne manager now"