ROS -> iOS working

- ros steering visualization now publishes Float32MultiArray message for iOS visualization

ros/src/autopilot/CMakeLists.txt

@@ -52,11 +52,9 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+#add_message_files(
+#  FILES
+#)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -73,10 +71,11 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   rosserial_msgs#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  rosserial_msgs
+  std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##

ros/src/autopilot/scripts/visualization/visualization.py

@@ -20,7 +20,7 @@
 import rospy
 from sensor_msgs.msg import Image
 from std_msgs.msg import Float32
-from std_msgs.msg import Int8
+from std_msgs.msg import Float32MultiArray
 from cv_bridge import CvBridge, CvBridgeError
 
 
@@ -96,7 +96,7 @@ def visualize_line(self, img, speed_ms, angle_steers, color=(0, 0, 255)):
         path_y, _ = self.calc_lookahead_offset(speed_ms, angle_steers, path_x)
         self.draw_path(img, path_x, path_y, color)
 
-        return img
+        return img, path_x, path_y
 
     def visualize_steering_wheel(self, image, angle):
 
@@ -153,8 +153,7 @@ def camera_update_callback(self, data):
         except CvBridgeError as e:
             raise e
 
-        if self.camera_select == 0:
-            self.current_frame = cv_image
+        self.current_frame = cv_image
 
     def steering_cmd_callback(self, data):
 
@@ -174,7 +173,6 @@ def __init__(self):
         self.bridge = CvBridge()
         self.steering_angle = 0.0
         self.cruise_cmds = 0.0
-        self.camera_select = -1
 
         # -----------------------------------------
         rospy.Subscriber('/vehicle/sensor/camera/front/image_color', Image,
@@ -183,16 +181,28 @@ def __init__(self):
 
         rospy.Subscriber('/vehicle/dbw/steering_cmds', Float32, callback=self.steering_cmd_callback)
         rospy.Subscriber('/vehicle/dbw/cruise_cmds', Float32, callback=self.cruise_callback)
+
         steering_viz_pub = rospy.Publisher('/visual/autopilot/angle_img', Image, queue_size=5)
         accel_viz_pub = rospy.Publisher('/visual/autopilot/cruise_img', Image, queue_size=5)
+        steering_ios_path = rospy.Publisher('/visual/ios/steering/path', Float32MultiArray, queue_size=5)
 
         rate = rospy.Rate(24)
 
         while not rospy.is_shutdown():
 
-            if self.current_frame is not None and self.camera_select != -1:
+            if self.current_frame is not None:
+
                 # Apply Steering Visualization #  -0.025
-                image = self.visualize_line(img=self.current_frame.copy(), angle_steers=self.steering_angle * -0.03, speed_ms=5)
+                image, path_x, path_y = self.visualize_line(img=self.current_frame.copy(),
+                                                            angle_steers=self.steering_angle * -0.035,
+                                                            speed_ms=5)
+
+                # Generate steering path message
+                path_msg = Float32MultiArray()
+                path_msg.data = np.hstack([path_x, path_y])
+                steering_ios_path.publish(path_msg)
+
+                # Generate steering image message
                 img_msg = self.bridge.cv2_to_imgmsg(image, "bgr8")
                 steering_viz_pub.publish(img_msg)
 

ros/src/driver/launch/display.launch

@@ -1,3 +1,4 @@
+
 <launch>
 
 	<!-- upload urdf -->
@@ -24,7 +25,7 @@
         <param name="map_url" value="$(arg url)" />
     </node>
 
-    <!-- ========= initialize GPS & localization modules ===+===== -->
+    <!-- ========= initialize GPS & localization modules ========= -->
 
     <include file="$(find localization)/launch/localization.launch"/>
 

ros/src/gui/launch/rqt.launch

@@ -3,12 +3,13 @@
     <!-- start camera -->
     <include file="$(find cv_camera)/launch/start_camera.launch"/>
 
+    <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" />
+
     <!-- setup the Arduino serial
     <node name="serial_node_1" pkg="rosserial_python" type="serial_node.py">
     	<param name="port" value="/dev/ttyACM0" />
     </node>
--->
-    <!-- setup the Arduino (braking accelerator) serial
+
     <node name="serial_node_2" pkg="rosserial_python" type="serial_node.py">
     	<param name="port" value="/dev/ttyACM1" />
     </node>

ros/src/segmentation/launch/segmentation.launch

@@ -19,6 +19,7 @@ All Rights Reserved.
         <param name="output_img_width" value="480"/>
         <param name="visualization" value="1" type="bool"/>
         <param name="weight_path" value="$(find segmentation)/scripts/weights/icnet_large_full_040_0.781.h5"/>
+        <!-- icnet_large_full_2_025_0.796.h5 -->
     </node>
 
 </launch>

ros/src/simulation/scripts/camera_sim_node

@@ -15,7 +15,7 @@ Contact: contact@neilnie.com
 """
 
 import cv2
-import pandas as pd
+import numpy as np
 import os
 import rospy
 from sensor_msgs.msg import Image
@@ -31,17 +31,16 @@ class CameraSimNode():
         self.bridge = CvBridge()
 
         self.camera_pub = rospy.Publisher('/cv_camera_node/image_sim', Image, queue_size=10)
+        # self.camera_vis_pub1 = rospy.Publisher('/cv_camera_node/image_sim_vis1', Image, queue_size=10)
+        # self.camera_vis_pub2 = rospy.Publisher('/cv_camera_node/image_sim_vis2', Image, queue_size=10)
+        # self.camera_vis_pub3 = rospy.Publisher('/cv_camera_node/image_sim_vis3', Image, queue_size=10)
         # self.sim_angle_pub = rospy.Publisher("/simulation/dbw/steering_cmds", Float32, queue_size=10)
 
         frame_path = rospy.get_param("/camera_sim_node/sim_data_path")
-        time_delay = rospy.get_param("/camera_sim_node/time_delay")
         self.rate = rospy.Rate(8)
 
-        # The csv data is read below. It must have two columns. the first column is image name,
-        # second frame steering angle the csv file contains many nonexisting frame information
-        # which is disregarded in the flowing loop.
-
         rospy.loginfo("Simulated Camera Feed Initialized. Publisher path: '/cv_camera_node/image_sim'")
+
         while not rospy.is_shutdown():
 
             for i in range(len(os.listdir(frame_path))):
@@ -50,12 +49,22 @@ class CameraSimNode():
                 # check to see if the file exists
                 if os.path.isfile(im_path):
                     frame = cv2.imread(im_path)
+
+                    b_channel, g_channel, r_channel = cv2.split(cv2.resize(frame, (160, 120)))
+                    img_rgba = cv2.merge((r_channel, g_channel, b_channel))
+
                     try:
                         img_msg = self.bridge.cv2_to_imgmsg(frame, "bgr8")
+                        img_msg_vis = Image()
+                        img_msg_vis.data = img_rgba.flatten().tolist()
+
                     except CvBridgeError as e:
                         raise e
                     # publish image
                     self.camera_pub.publish(img_msg)
+                    # self.camera_vis_pub1.publish(img_msg_vis)
+                    # self.camera_vis_pub2.publish(img_msg_vis)
+                    # self.camera_vis_pub3.publish(img_msg_vis)
 
                 # ros rate sleep
                 self.rate.sleep()