Anul 3 Semestrul 1

2025-02-06 20:33:26 +02:00
parent 0b130ee18c
commit 184f3bd92e
313 changed files with 348499 additions and 0 deletions
@@ -0,0 +1,2 @@
+devel/
+build/
@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration
@@ -0,0 +1,69 @@
+# toplevel CMakeLists.txt for a catkin workspace
+# catkin/cmake/toplevel.cmake
+
+cmake_minimum_required(VERSION 3.0.2)
+
+project(Project)
+
+set(CATKIN_TOPLEVEL TRUE)
+
+# search for catkin within the workspace
+set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
+execute_process(COMMAND ${_cmd}
+  RESULT_VARIABLE _res
+  OUTPUT_VARIABLE _out
+  ERROR_VARIABLE _err
+  OUTPUT_STRIP_TRAILING_WHITESPACE
+  ERROR_STRIP_TRAILING_WHITESPACE
+)
+if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
+  # searching fot catkin resulted in an error
+  string(REPLACE ";" " " _cmd_str "${_cmd}")
+  message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
+endif()
+
+# include catkin from workspace or via find_package()
+if(_res EQUAL 0)
+  set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
+  # include all.cmake without add_subdirectory to let it operate in same scope
+  include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
+  add_subdirectory("${_out}")
+
+else()
+  # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
+  # or CMAKE_PREFIX_PATH from the environment
+  if(NOT DEFINED CMAKE_PREFIX_PATH)
+    if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
+      if(NOT WIN32)
+        string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      else()
+        set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      endif()
+    endif()
+  endif()
+
+  # list of catkin workspaces
+  set(catkin_search_path "")
+  foreach(path ${CMAKE_PREFIX_PATH})
+    if(EXISTS "${path}/.catkin")
+      list(FIND catkin_search_path ${path} _index)
+      if(_index EQUAL -1)
+        list(APPEND catkin_search_path ${path})
+      endif()
+    endif()
+  endforeach()
+
+  # search for catkin in all workspaces
+  set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
+  find_package(catkin QUIET
+    NO_POLICY_SCOPE
+    PATHS ${catkin_search_path}
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+  unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
+
+  if(NOT catkin_FOUND)
+    message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
+  endif()
+endif()
+
+catkin_workspace()
@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(pkg_1)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * 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
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES pkg_1
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/pkg_1.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/pkg_1_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_pkg_1.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>pkg_1</name>
+  <version>0.0.0</version>
+  <description>The pkg_1 package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="pi@todo.todo">pi</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/pkg_1</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
@@ -0,0 +1,2 @@
+#!/usr/bin/env python3
+print("Hello World")
@@ -0,0 +1,28 @@
+#!/usr/bin/env python3
+import rospy
+import cv2
+import numpy as np
+from sensor_msgs.msg import Image
+ROS_NODE_NAME = "image_processing_node"
+def img_process(img):
+    rospy.loginfo("image width: %s height: %s" % (img.width, img.height))
+    frame = np.ndarray(shape=(img.height, img.width, 3), dtype=np.uint8, buffer=img.data)
+    center_coordinates = (img.width // 2, img.height // 2)  # center of the image
+    radius = min(img.width, img.height) // 10               # radius as 1/10th of the smaller dimension
+    color = (0, 255, 0)                                     # green color in BGR
+    thickness = 2                                           # thickness of the circle outline
+    cv2_img = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
+    cv2.circle(cv2_img, center_coordinates, radius, color, thickness)
+    cv2.imshow("Frame", cv2_img)
+    cv2.waitKey(1) #this forces the window opened by OpenCV to remain open
+def cleanup():
+    rospy.loginfo("Shutting down...")
+    cv2.destroyWindow("Frame")
+if __name__ == "__main__":
+    rospy.init_node(ROS_NODE_NAME, log_level=rospy.INFO)
+    rospy.on_shutdown(cleanup)
+    rospy.Subscriber("/usb_cam/image_raw", Image, img_process)
+try:
+    rospy.spin()
+except KeyboardInterrupt:
+    pass
@@ -0,0 +1,24 @@
+#!/usr/bin/env python3
+import rospy
+import cv2
+import numpy as np
+from sensor_msgs.msg import Image
+
+ROS_NODE_NAME = "image_processing_node"
+def img_process(img):
+    rospy.loginfo("image width: %s height: %s" % (img.width, img.height))
+    frame = np.ndarray(shape=(img.height, img.width, 3), dtype=np.uint8, buffer=img.data)
+    cv2_img = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
+    cv2.imshow("Frame", cv2_img)
+    cv2.waitKey(1) #this forces the window opened by OpenCV to remain open
+def cleanup():
+    rospy.loginfo("Shutting down...")
+    cv2.destroyWindow("Frame")
+if __name__ == "__main__":
+    rospy.init_node(ROS_NODE_NAME, log_level=rospy.INFO)
+    rospy.on_shutdown(cleanup)
+    rospy.Subscriber("/usb_cam/image_raw", Image, img_process)
+    try:
+        rospy.spin()
+    except KeyboardInterrupt:
+        pass
@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+import rospy
+import cv2
+import numpy as np
+from sensor_msgs.msg import Image
+
+ROS_NODE_NAME = "image_processing_node"
+def img_process(img):
+    frame = np.ndarray(shape=(img.height, img.width, 3), dtype=np.uint8, buffer=img.data)
+    color = (0, 255, 0)                                     # green color in BGR
+    detect_largest_blob(frame)
+
+    #cv2_img = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
+    #cv2.circle(cv2_img, center_coordinates, radius, color, thickness)
+    #cv2.imshow("Frame", cv2_img)
+    #cv2.waitKey(1) #this forces the window opened by OpenCV to remain open
+
+def detect_largest_blob(image):
+        # Convert BGR to HSV
+        hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)
+        lower_color = np.array([0, 100, 100])
+        upper_color = np.array([10, 255, 255])
+
+        mask = cv2.inRange(hsv, lower_color, upper_color)
+
+        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+        if contours:
+            # Find the largest contour
+            largest_contour = max(contours, key=cv2.contourArea)
+
+            # Draw the bounding box
+            x, y, w, h = cv2.boundingRect(largest_contour)
+            cv2.rectangle(image, (x, y), (x + w, y + h), (0, 255, 0), 2)
+        # Display tihe result
+        color = cv2.cvtColor(image,cv2.COLOR_BGR2RGB)
+        cv2.imshow('Detected Blob', color)
+        cv2.waitKey(1)
+
+def cleanup():
+    rospy.loginfo("Shutting down...")
+    cv2.destroyWindow("Frame")
+if __name__ == "__main__":
+    rospy.init_node(ROS_NODE_NAME, log_level=rospy.INFO)
+    rospy.on_shutdown(cleanup)
+    rospy.Subscriber("/usb_cam/image_raw", Image, img_process)
+try:
+    rospy.spin()
+except KeyboardInterrupt:
+    pass
+
@@ -0,0 +1,54 @@
+#!/usr/bin/env python3 
+import rospy
+import cv2
+import math
+import numpy as np
+import time
+from std_srvs.srv import Empty
+from puppy_control.msg import Pose, Gait, Velocity
+
+ROS_NODE_NAME = "move_node"
+
+# some default values for the pose, gait and velocity
+# depending on what you want to do, these values should be changed
+pose_msg = Pose(roll=math.radians(0), pitch=math.radians(0), yaw=0, height=-15, x_shift=0, stance_x=0, stance_y=0, run_time=500)
+gait_msg = Gait(overlap_time=0.2, swing_time=0.2, clearance_time=0.2, z_clearance=3)
+vel_msg = Velocity(x=0, y=0, yaw_rate=math.radians(0))
+def cleanup():
+    rospy.loginfo("Shutting down...")
+    # when closing, reset the servos
+    rospy.ServiceProxy("/puppy_control/go_home", Empty)()
+
+if __name__ == "__main__":
+    rospy.init_node(ROS_NODE_NAME, log_level=rospy.INFO)
+    rospy.on_shutdown(cleanup)
+    # first reset the servos
+    rospy.ServiceProxy("/puppy_control/go_home", Empty)()
+    pose_pub = rospy.Publisher("/puppy_control/pose", Pose, queue_size=1)
+    gait_pub = rospy.Publisher("/puppy_control/gait", Gait, queue_size=1)
+    vel_pub = rospy.Publisher("/puppy_control/velocity", Velocity, queue_size=1)
+
+    # set the gait
+    # this will remain at this value until a new gait type is specified
+    gait_pub.publish(gait_msg)
+    time.sleep(0.2)
+    i = 0
+    # set the pose
+    # similar to the gait, the pose is set and will remain like this until specified otherwise
+    pose_pub.publish(pose_msg)
+    time.sleep(0.2)
+    while not rospy.is_shutdown():
+        # let the robot run for 10 steps (this was done at the lecture because the amount of space was limited)
+        # normally, we don't need to count our steps but estimate how close we are to our target
+        i = i + 1
+        if i < 10:
+            # x axis is forward or backward movement (positive for forward)
+            vel_msg.x = 10
+            # yaw rate is for steering to the left or right (positive for left)
+            vel_msg.yaw_rate = math.radians(10)
+        else:
+            vel_msg.x = 0
+            vel_msg.yaw_rate = 0
+        vel_pub.publish(vel_msg)
+        # wait for 1 second 
+        time.sleep(1)
@@ -0,0 +1,128 @@
+#!/usr/bin/env python3 
+import rospy
+import cv2
+import math
+import numpy as np
+import time
+from threading import Thread, Lock
+from sensor_msgs.msg import Image
+from std_srvs.srv import Empty
+from puppy_control.msg import Pose, Gait, Velocity
+from puppy_control.srv import SetRunActionName
+
+ROS_NODE_NAME = "move_on_detect_node"
+
+lock = Lock()
+move_th = None
+
+max_contour = None
+contour_center = None
+radius = 0
+
+surface = 0
+
+pose_pub = None
+gait_pub = None
+vel_pub = None
+
+shut = False
+pose_msg = Pose(roll=math.radians(0), pitch=math.radians(0), yaw=0, height=-10, x_shift=0, stance_x=0, stance_y=0, run_time=500)
+gait_msg = Gait(overlap_time=0.3, swing_time=0.5, clearance_time=0.0, z_clearance=3)
+vel_msg = Velocity(x=0, y=0, yaw_rate=math.radians(0))
+
+def detect_largest_blob(image):
+        global largest_contour
+        # Convert BGR to HSV
+        hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)
+        lower_color = np.array([0, 100, 100])
+        upper_color = np.array([10, 255, 255])
+
+        mask = cv2.inRange(hsv, lower_color, upper_color)
+
+        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+        if contours:
+            # Find the largest contour
+            largest_contour = max(contours, key=cv2.contourArea)
+
+            # Draw the bounding box
+            x, y, w, h = cv2.boundingRect(largest_contour)
+            cv2.rectangle(image, (x, y), (x + w, y + h), (0, 255, 0), 2)
+            if w<50 or h<50:
+                x,y,w,h,largest_contour=0,0,0,0,None
+        else:
+            x,y,w,h,largest_contour=0,0,0,0,None
+        # Display tihe result
+        color = cv2.cvtColor(image,cv2.COLOR_BGR2RGB)
+        #cv2.imshow('Detected Blob', color)
+        #cv2.waitKey(1)
+        return largest_contour,x,y,w,h 
+
+def img_process(img):
+    global pose_pub, pose_msg
+    global contour_center, radius, max_contour
+    global lock, shut, surface
+    frame = np.ndarray(shape=(img.height, img.width, 3), dtype=np.uint8, buffer=img.data)
+    cv2_img = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
+    # find all contours
+    with lock:
+        if not shut:
+            #max_contour = get max contour and store in this variable
+            max_contour, x, y,w,h = detect_largest_blob(cv2_img)
+            if max_contour is not None:
+               # print(max_contour)
+               contour_center = (x+w //2,y+h //2)
+            surface = x * y
+                # calculate the center of the contour and estimate the size of the contour               
+                # draw the bounding circle or box around the contour
+        cv2.imshow("Frame", cv2_img)
+        cv2.waitKey(1)
+
+def cleanup():
+    global shut, lock
+    with lock:
+        shut = True
+    rospy.loginfo("Shutting down...")
+    cv2.destroyWindow("Frame")
+    rospy.ServiceProxy("/puppy_control/go_home", Empty)()
+
+def move():
+    global pose_pub, vel_pub
+    global contour_center, radius
+    global max_contour
+    global lock, shut
+    while True:
+        time.sleep(0.2)
+        with lock:
+            if shut:
+                break
+            if max_contour is not None:
+                # if there is a contour, decide how to move and change pitch
+                vel_msg.x = 10
+                vel_pub.publish(vel_msg)
+
+            elif surface < 10:
+                vel_msg.x = 0
+                vel_pub.publish(vel_msg)
+            elif surface > 100:
+                vel_msg.x = 0
+                vel_pub.publish(vel_msg)
+                # if no contour is detected, do something else
+
+if __name__ == "__main__":
+    rospy.init_node(ROS_NODE_NAME, log_level=rospy.INFO)
+    rospy.on_shutdown(cleanup)
+    
+    pose_pub = rospy.Publisher("/puppy_control/pose", Pose, queue_size=1)
+    gait_pub = rospy.Publisher("/puppy_control/gait", Gait, queue_size=1)
+    vel_pub = rospy.Publisher("/puppy_control/velocity", Velocity, queue_size=1)
+    
+    gait_pub.publish(gait_msg)
+    
+    rospy.Subscriber("/usb_cam/image_raw", Image, img_process)
+    rospy.ServiceProxy("/puppy_control/go_home", Empty)()
+    # create a daemon that will run the "move" function in the background
+    # the move function should contain all the logic for moving the robot towards the detected object and for tracking it
+    move_th = Thread(target=move, daemon=True)
+    move_th.start()
+    rospy.spin()
@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+import rospy
+from sensor.msg import Led, RGB
+
+ROS_NODE_NAME = "roseNode1"
+led_pub = None
+
+def changeColor():
+	global led_pub
+	led_pub = rospy.Publisher("/sensor/rgb_led", Led, queue_size=1)
+	colors = [(255,0,0), (255,100,0), (255,255,0),(0,255,0),(0,0,255),(75,0,130)]
+	size=len(colors)
+	i=0
+	rate=rospy.Rate(1)
+	led=Led()
+	led.index=0
+	while not rospy.is_shutdown():
+		led.rgb.r = colors[i][0]
+		led.rgb.g = colors[i][1]
+		led.rgb.b = colors[i][2]
+		led_pub.publish(led)
+		i=(i+1)%size
+		rate.sleep()
+
+
+def cleanup():
+	global led_pub
+	led = Led(0, RGB(0,0,0))
+	if led_pub !=None:
+		led_pub.publish(led)
+	rospy.loginfo("hehe")
+
+if __name__=='__main__':
+	rospy.init_node(ROS_NODE_NAME, log_level=rospy.INFO)
+	rospy.on_shutdown(cleanup)
+	try:
+		changeColor()
+	except KeyboardInterrupt:
+		pass
				`@@ -0,0 +1 @@`
				`# This file currently only serves to mark the location of a catkin workspace for tool integration`