prepare template

Dockerfile

@@ -3,14 +3,7 @@ ARG REPO_NAME="template-ros-core"
 
 # ==================================================>
 # ==> Do not change this code
-ARG ARCH=arm32v7
-ARG MAJOR=daffy
-ARG BASE_TAG=${MAJOR}-${ARCH}
-ARG BASE_IMAGE=dt-core
-
-# define base image
-FROM duckietown/${BASE_IMAGE}:${BASE_TAG}
-
+FROM light5551/ado_torch_template:arm64
 # define repository path
 ARG REPO_NAME
 ARG REPO_PATH="${CATKIN_WS_DIR}/src/${REPO_NAME}"

packages/circle_drive/launch/circle_drive.launch

@@ -2,6 +2,7 @@
 	<arg name="veh" default="$(env VEHICLE_NAME)"/>
 	<group ns="$(arg veh)">
 		<remap from="circle_drive/car_cmd" to="joy_mapper_node/car_cmd"/>
+		<!-- <remap from="circle_drive/image" to="camera_node/image/compressed"/> -->
 		<node name="circle_drive" pkg="circle_drive" type="circle_drive.py" output="screen" required="true"/>
 <!--		<node name="recorder" pkg="rosbag" type="record" args="record -o /data/bags /autobot04/camera_node/image/compressed" /> -->
 	</group>

packages/circle_drive/scripts/circle_drive.py

@@ -3,28 +3,43 @@
 import rospy
 from duckietown.dtros import DTROS, NodeType
 from duckietown_msgs.msg import Twist2DStamped
+from sensor_msgs.msg import CompressedImage
+from cv_bridge import CvBridge
+from solution import solution
+
 
 class MyNode(DTROS):
 
     def __init__(self, node_name):
         super(MyNode, self).__init__(node_name=node_name, node_type=NodeType.DEBUG)
         self.pub = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1)
+        self.bridge = CvBridge()
+        self.cur_img = None
+        self.sub_image = rospy.Subscriber(
+            "/autobot20/camera_node/image/compressed",
+            #"~image",
+            CompressedImage,
+            self.action,
+            queue_size=1
+        )
 
     def run(self):
-        # publish message every 1 second
-        rate = rospy.Rate(1) # 1Hz
-        while not rospy.is_shutdown():
-            msg = Twist2DStamped()
-            msg.v = 0.0
-            msg.omega = 5.0
-            rospy.loginfo("Publishing message 0/0.5")
-            self.pub.publish(msg)
-            rate.sleep()
-            msg.omega = 0.0
-            rospy.loginfo("Publishing message 0/0.0")
-            self.pub.publish(msg)
-            rate.sleep()
-            sys.stdout.flush()
+        pass
+
+    def action(self, image_msg):
+        try:
+            image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
+            self.cur_img = image
+
+        except ValueError as e:
+            self.logerr('Could not decode image: %s' % e)
+            return
+        vel, omega = solution(self.cur_img)
+        msg = Twist2DStamped()
+        msg.v = vel
+        msg.omega = omega
+        self.pub.publish(msg)
+        sys.stdout.flush()
 
 if __name__ == '__main__':
     # create the node

packages/circle_drive/scripts/solution.py

@@ -0,0 +1,4 @@
+
+def solution(obs):
+    print(obs.shape)
+    return [0.1, 1]