Merge branch 'zedTopic' into parameterized-stop

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>
 
 
@@ -44,13 +48,13 @@
         <node name="obstacle_detector" pkg="cart_endpoints" type="obstacle_detector.py" output="screen"/>
         <node name="collision_detector" pkg="cart_planning" type="collision_detector.py" output="screen"/>
     </group>
-    <include file="$(find video_stream_opencv)/launch/camera.launch">
-            <!--This is referring to the zed camera facing the passenger-->
+    <!-- <include file="$(find video_stream_opencv)/launch/camera.launch">
+            This is referring to the zed camera facing the passenger
             <arg name = "camera_name" value="zed"/>
             <arg name="video_stream_provider" value="/dev/v4l/by-id/usb-Leopard_ZED-video-index0
 " />
             <arg name="visualize" value="$(arg visualize_pose)" />
-        </include>
+        </include> -->
     <include file="$(find video_stream_opencv)/launch/camera.launch">
             <arg name="camera_name" value="front_facing"/>
             <arg name="video_stream_provider" value="/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._H264_USB_Camera_SN0001-video-index0" />

cart_endpoints/scripts/network_node.py

@@ -196,36 +196,36 @@ def status_update(data):
 last_front_pub = -1
 last_passenger_pub = -1
 
-def passenger_image_callback(img_msg):
-    global last_passenger_pub
-    cur_time = time.time()
-    if cur_time > last_passenger_pub + 1:
-        bridge = CvBridge() 
-        image_raw = np.frombuffer(img_msg.data, dtype=np.uint8)
-        image_raw.shape = (img_msg.height, img_msg.width, 3)
-        image_raw = bridge.imgmsg_to_cv2(img_msg, desired_encoding="mono8")
-        h,w = image_raw.shape[:2]
-        # Crop image and get the image width and height  
-        cropped = image_raw[0:h, 0:672]  
-        final_image = cv2.flip(cropped, -1)
-        dim = (400, 400)
-        f2 = cv2.resize(final_image, dim, interpolation=cv2.INTER_AREA)
-        retval, buffer = cv2.imencode('.jpg', f2)    
-        send('passenger-video',base64.b64encode(buffer) )
-        last_passenger_pub = cur_time
+# def passenger_image_callback(img_msg):
+#     global last_passenger_pub
+#     cur_time = time.time()
+#     if cur_time > last_passenger_pub + 1:
+#         bridge = CvBridge() 
+#         image_raw = np.frombuffer(img_msg.data, dtype=np.uint8)
+#         image_raw.shape = (img_msg.height, img_msg.width, 3)
+#         image_raw = bridge.imgmsg_to_cv2(img_msg, desired_encoding="mono8")
+#         h,w = image_raw.shape[:2]
+#         # Crop image and get the image width and height  
+#         cropped = image_raw[0:h, 0:672]  
+#         final_image = cv2.flip(cropped, -1)
+#         dim = (400, 400)
+#         f2 = cv2.resize(final_image, dim, interpolation=cv2.INTER_AREA)
+#         retval, buffer = cv2.imencode('.jpg', f2)    
+#         send('passenger-video',base64.b64encode(buffer) )
+#         last_passenger_pub = cur_time
 
 
 
-def front_image_callback(img_msg):
-    global last_front_pub
-    cur_time = time.time()
-    if cur_time > last_front_pub + 1:
-        cur_time = time.time()
-        bridge = CvBridge() 
-        image_raw = bridge.imgmsg_to_cv2(img_msg, desired_encoding="mono8")
-        retval, buffer = cv2.imencode('.jpg', image_raw)    
-        send('cart-video', base64.b64encode(buffer))
-        last_front_pub = cur_time
+# def front_image_callback(img_msg):
+#     global last_front_pub
+#     cur_time = time.time()
+#     if cur_time > last_front_pub + 1:
+#         cur_time = time.time()
+#         bridge = CvBridge() 
+#         image_raw = bridge.imgmsg_to_cv2(img_msg, desired_encoding="mono8")
+#         retval, buffer = cv2.imencode('.jpg', image_raw)    
+#         send('cart-video', base64.b64encode(buffer))
+#         last_front_pub = cur_time
 
 def eta_callback(msg):
     send('arrived-time', msg.data)
@@ -248,8 +248,8 @@ def eta_callback(msg):
     safety_exit_pub = rospy.Publisher('/safety_exit', Bool, queue_size=10)
 
 
-    rospy.Subscriber('/zed/image_raw', Image, passenger_image_callback)
-    rospy.Subscriber('/front_facing/image_raw', Image, front_image_callback)
+    # rospy.Subscriber('/zed/image_raw', Image, passenger_image_callback)
+    # rospy.Subscriber('/front_facing/image_raw', Image, front_image_callback)
     rospy.Subscriber('/current_position', Int8, send_position_index)
     rospy.Subscriber('/vehicle_state', VehicleState, status_update)
     rospy.Subscriber('/pullover', Bool, pullover_callback)

cart_planning/launch/constants.launch

@@ -11,7 +11,7 @@
         test_loc_local: [67.6, 115]
 
         <!-- Global Planner graph file parameter -->
-        graph_file: "/home/jeffercize/catkin_ws/src/ai-navigation/EastCampusLiteV6.gml"
+        graph_file: "/home/jacart/catkin_ws/src/ai-navigation/EastCampusLiteV6.gml"
 
         <!-- Safety Parameters -->
         <!-- These are reactive in the sense that if the max_speed is reached the cart will emergency stop -->

cart_simulator/testing_description/urdf/4_point_model.urdf.xacro

@@ -32,7 +32,7 @@
       <origin xyz=".1 -.1 -0.66" rpy="0 0 1.57" />
       <geometry>
         <!-- <box size="2.337 1.27 1.778"/> -->
-        <mesh filename="file:///home/jeffercize/catkin_ws/src/ai-navigation/golf_cart/meshes/golf_cart_blender.dae" scale="1.25 1.25 1.25"/>
+        <mesh filename="file:///home/jacart/catkin_ws/src/ai-navigation/golf_cart/meshes/golf_cart_blender.dae" scale="1.25 1.25 1.25"/>
       </geometry>
       <material name="silver">
         <color rgba="0.75 0.75 0.75 1"/>

cart_util/test_live_analysis_v2.py

@@ -20,7 +20,7 @@
 import json
 
 # Add openpose to system PATH and import
-sys.path.append('/home/jeffercize/catkin_ws/src/openpose/build/python');
+sys.path.append('/home/jacart/catkin_ws/src/openpose/build/python');
 from openpose import pyopenpose as op
 
 #######################
@@ -48,7 +48,7 @@
 ##############
 # Custom Params (refer to include/openpose/flags.hpp for more parameters)
 params = dict()
-params["model_folder"] = "/home/jeffercize/catkin_ws/src/openpose/models/"
+params["model_folder"] = "/home/jacart/catkin_ws/src/openpose/models/"
 
 # Starting OpenPose
 opWrapper = op.WrapperPython()

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