diff --git a/Dockerfile b/Dockerfile index c5e59c27..567d5fc1 100644 --- a/Dockerfile +++ b/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}" diff --git a/packages/circle_drive/launch/circle_drive.launch b/packages/circle_drive/launch/circle_drive.launch index 4cde3d2d..90cfe775 100644 --- a/packages/circle_drive/launch/circle_drive.launch +++ b/packages/circle_drive/launch/circle_drive.launch @@ -2,6 +2,7 @@ + diff --git a/packages/circle_drive/scripts/circle_drive.py b/packages/circle_drive/scripts/circle_drive.py index 3db98995..e811159b 100755 --- a/packages/circle_drive/scripts/circle_drive.py +++ b/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 diff --git a/packages/circle_drive/scripts/solution.py b/packages/circle_drive/scripts/solution.py new file mode 100644 index 00000000..297e0ab0 --- /dev/null +++ b/packages/circle_drive/scripts/solution.py @@ -0,0 +1,4 @@ + +def solution(obs): + print(obs.shape) + return [0.1, 1]