diff --git a/builder/test/tests.py b/builder/test/tests.py index d453d505..6bce4823 100755 --- a/builder/test/tests.py +++ b/builder/test/tests.py @@ -2,6 +2,7 @@ # validate all required modules installed +import os import rospy from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import Range, BatteryState @@ -32,9 +33,11 @@ import tf2_geometry_msgs import VL53L1X import pymavlink from pymavlink import mavutil -import rpi_ws281x -import pigpio # from espeak import espeak from pyzbar import pyzbar print(cv2.getBuildInformation()) + +if not os.environ.get('VM'): + import rpi_ws281x + import pigpio diff --git a/builder/test/tests.sh b/builder/test/tests.sh index 04883e36..0f016714 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -6,16 +6,10 @@ set -ex # validate required software is installed -python --version python2 --version python3 --version -ipython --version ipython3 --version -# ptvsd does not have a stand-alone binary -python -m ptvsd --version -python3 -m ptvsd --version - node -v npm -v @@ -25,47 +19,64 @@ lsof -v git --version vim --version pip --version -pip2 --version pip3 --version tcpdump --version monkey --version -pigpiod -v -i2cdetect -V -butterfly -h # espeak --version -mjpg_streamer --version systemctl --version +if [ -z $VM ]; then + # rpi only software + python --version + ipython --version + pip2 --version + # `python` is python2 for now + [[ $(python -c 'import sys;print(sys.version_info.major)') == "2" ]] + + # ptvsd does not have a stand-alone binary + python -m ptvsd --version + python3 -m ptvsd --version + + pigpiod -v + i2cdetect -V + butterfly -h + mjpg_streamer --version +fi + # ros stuff roscore -h rosversion clover rosversion aruco_pose -rosversion vl53l1x rosversion mavros rosversion mavros_extras rosversion ws281x rosversion led_msgs rosversion dynamic_reconfigure rosversion tf2_web_republisher -rosversion compressed_image_transport -rosversion rosbridge_suite -rosversion rosserial +rosversion rosbridge_server rosversion usb_cam rosversion cv_camera rosversion web_video_server -rosversion rosshow rosversion nodelet rosversion image_view -# validate some versions -[[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix [[ $(rosversion ws281x) == "0.0.13" ]] +if [ -z $VM ]; then + rosversion compressed_image_transport + rosversion rosshow + rosversion vl53l1x + rosversion rosserial + [[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix +fi + +[ $VM ] && H="/home/clover" || H="/home/pi" + # validate examples are present -[[ $(ls /home/pi/examples/*) ]] +[[ $(ls $H/examples/*) ]] # validate web tools present -[ -d /home/pi/.ros/www ] -[ "$(readlink /home/pi/.ros/www/clover)" = "/home/pi/catkin_ws/src/clover/clover/www" ] -[ "$(readlink /home/pi/.ros/www/clover_blocks)" = "/home/pi/catkin_ws/src/clover/clover_blocks/www" ] +[ -d $H/.ros/www ] +[ "$(readlink $H/.ros/www/clover)" = "$H/catkin_ws/src/clover/clover/www" ] +[ "$(readlink $H/.ros/www/clover_blocks)" = "$H/catkin_ws/src/clover/clover_blocks/www" ] diff --git a/clover/examples/camera.py b/clover/examples/camera.py new file mode 100644 index 00000000..67d805ee --- /dev/null +++ b/clover/examples/camera.py @@ -0,0 +1,64 @@ +# Information: https://clover.coex.tech/camera + +# Example on basic working with the camera and image processing: + +# - cuts out a central square from the camera image; +# - publishes this cropped image to the topic `/cv/center`; +# - computes the average color of it; +# - prints its name to the console. + +import rospy +import cv2 +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +from clover import long_callback + +rospy.init_node('cv') +bridge = CvBridge() + +printed_color = None +center_pub = rospy.Publisher('~center', Image, queue_size=1) + +def get_color_name(h): + if h < 15: return 'red' + if h < 30: return 'orange' + elif h < 60: return 'yellow' + elif h < 90: return 'green' + elif h < 120: return 'cyan' + elif h < 150: return 'blue' + elif h < 170: return 'magenta' + else: return 'red' + + +@long_callback +def image_callback(msg): + img = bridge.imgmsg_to_cv2(msg, 'bgr8') + + # convert to HSV to work with color hue + img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + + # cut out a central square + w = img.shape[1] + h = img.shape[0] + r = 20 + center = img_hsv[h // 2 - r:h // 2 + r, w // 2 - r:w // 2 + r] + + # compute and print the average hue + mean_hue = center[:, :, 0].mean() + color = get_color_name(mean_hue) + global printed_color + if color != printed_color: + print(color) + printed_color = color + + # publish the cropped image + center = cv2.cvtColor(center, cv2.COLOR_HSV2BGR) + center_pub.publish(bridge.cv2_to_imgmsg(center, 'bgr8')) + +# process every frame: +image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1) + +# process 5 frames per second: +# image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1) + +rospy.spin() diff --git a/clover/launch/aruco.launch b/clover/launch/aruco.launch index 53766c05..4fbb5e44 100644 --- a/clover/launch/aruco.launch +++ b/clover/launch/aruco.launch @@ -53,4 +53,8 @@ + + + diff --git a/clover/launch/clover.launch b/clover/launch/clover.launch index 2afa96b0..22b3b9d6 100644 --- a/clover/launch/clover.launch +++ b/clover/launch/clover.launch @@ -48,8 +48,6 @@ - - diff --git a/clover_simulation/src/sim_leds.cpp b/clover_simulation/src/sim_leds.cpp index fb188af7..f281d99a 100644 --- a/clover_simulation/src/sim_leds.cpp +++ b/clover_simulation/src/sim_leds.cpp @@ -65,7 +65,8 @@ public: } role = (ros::this_node::getName() == "/gazebo") ? Role::Server : Role::Client; - ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s)", role == Role::Client ? "client" : "server"); + ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s) in namespace '%s'", + role == Role::Client ? "client" : "server", robotNamespace.c_str()); nh.reset(new ros::NodeHandle(robotNamespace)); @@ -109,7 +110,6 @@ LedController& get(std::string robotNamespace) std::lock_guard lock(controllerMutex); auto it = controllers.find(robotNamespace); if (it == controllers.end()) { - gzwarn << "Creating new LED controller for namespace " << robotNamespace << "\n"; controllers[robotNamespace].reset(new LedController(robotNamespace)); return *controllers[robotNamespace]; }