Merge branch 'master' into roswww-static-target

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

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" ]

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()

clover/launch/aruco.launch

@@ -53,4 +53,8 @@
         <param name="force_init" value="$(arg force_init)"/>
         <param name="offset_frame_id" value="aruco_map"/>
     </node>
+
+    <!-- run map_flipped frame if placement is ceiling -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame"
+        args="0 0 0 3.1415926 3.1415926 0 map map_flipped" if="$(eval placement == 'ceiling')"/>
 </launch>

clover/launch/clover.launch

@@ -48,8 +48,6 @@
         <param name="disable_on_vpe" value="false"/>
     </node>
 
-    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
-
     <!-- simplified offboard control -->
     <node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
         <param name="reference_frames/main_camera_optical" value="map"/>

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<std::mutex> 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];
 	}