1. object tracking
The above uses opencv to track facial key points and colors The result of tracking is the region of the target in the image, ROI region of interest. Posted in ROS topic /roi If the camera is mounted on a mobile site We can use the x_offset horizontal coordinate offset of the ROI (relative to the entire image) We can rotate and move the chassis to ensure that the x_offset of the ROI is located in the horizontal center of the image (To the left, rotate left, counterclockwise; to the right, rotate right, clockwise) If the camera adds a vertical servo, you can also rotate the servo to make the y_offset of the ROI Located in the vertical center of the image You can also use the depth information corresponding to the ROI area to control the movement of the chassis forward and backward. Deeper forward Less depth retreat Keep the depth value at a fixed value
Key program: rbx1_apps/nodes/object_tracker.py
1) First start a depth camera or ordinary camera depth camera 1 Microsoft Kinect: $ roslaunch freenect_launch freenect-registered-xyzrgb.launch
Depth camera 2 Asus Xtion, Xtion Pro, or Primesense 1.08/1.09 cameras: $ roslaunch openni2_launch openni2.launch depth_registration:=true webcam : $ roslaunch rbx1_vision usb_cam.launch video_device:=/dev/video0
2) Launch the face tracking node roslaunch rbx1_vision face_tracker2.launch
ran node/rbx1_vision/node/face_tracker2.py
3) Launch the target tracking node roslaunch rbx1_apps object_tracker.launch
ran nodes/rbx1_apps/nodes/object_tracker.py
4) Use rqt_plot to display face position and movement speed information rqt_plot /cmd_vel/angular/z
5) Test the tracking effect in the simulation environment
$ roslaunch rbx1_bringup fake_turtlebot.launch $ rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz Move the face and turtlebot will rotate
6) Code analysis /rbx1_apps/nodes/object_tracker.py
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import RegionOfInterest, CameraInfo
from geometry_msgs.msg import Twist
import thread
classObjectTracker():
def __init__(self):
rospy.init_node(“object_tracker”)
# Node shutdown Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Update frequency How often should we update the robot’s motion?
self.rate = rospy.get_param(“~rate”, 10)
r = rospy.Rate(self.rate)
# The maximum rotation speed of the mobile chassis The maximum rotation speed in radians per second
self.max_rotation_speed = rospy.get_param(“~max_rotation_speed”, 2.0)
# The minimum rotation speed of the mobile chassis The minimum rotation speed in radians per second
self.min_rotation_speed = rospy.get_param(“~min_rotation_speed”, 0.5)
# Sensitivity to target displacements. Setting this too high
# can lead to oscillations of the robot.
self.gain = rospy.get_param(“~gain”, 2.0) # Sensitivity, gain is equivalent to a proportional controller
# The x threshold (% of image width) indicates how far off-center
# the ROI needs to be in the x-direction before we react
self.x_threshold = rospy.get_param(“~x_threshold”, 0.1) # Offset threshold
# Publisher to control the robot’s movement Publish movement information control instructions
self.cmd_vel_pub = rospy.Publisher(‘cmd_vel’, Twist, queue_size=5)
# Initialize the movement command
self.move_cmd = Twist() #Initialize motion information control instructions
# Get a lock for updating the self.move_cmd values
self.lock = thread.allocate_lock() # Thread lock
# We will get the image width and height from the camera_info topic
self.image_width = 0
self.image_height = 0
# Set flag to indicate when the ROI stops updating
self.target_visible = False
# Wait for the camera_info topic to become available Wait
rospy.loginfo(“Waiting for camera_info topic…”)
rospy.wait_for_message(‘camera_info’, CameraInfo)
#Subscribe to the topic to get camera image information Subscribe the camera_info topic to get the image width and height
rospy.Subscriber(‘camera_info’, CameraInfo, self.get_camera_info, queue_size=1)
# Wait until we actually have the camera data
while self.image_width == 0 or self.image_height == 0:
rospy.sleep(1)
# Subscribe to the ROI topic and set the callback to update the robot’s motion
#The callback function is set_cmd_ve()
rospy.Subscriber(‘roi’, RegionOfInterest, self.set_cmd_vel, queue_size=1)
# Wait until we have an ROI to follow
rospy.loginfo(“Waiting for messages on /roi…”)
rospy.wait_for_message(‘roi’, RegionOfInterest)
rospy.loginfo(“ROI messages detected. Starting tracker…”)
# Start tracking loop ====
while not rospy.is_shutdown():
# Acquire a lock while we’re setting the robot speeds
self.lock.acquire() #Multi-thread lock
try:
# If the target is not visible, stop the robot
if not self.target_visible:
self.move_cmd = Twist()# The target is not seen in the field of view and does not move.
else:
# Reset the flag to False by default
self.target_visible = False
# Send the Twist command to the robot
self.cmd_vel_pub.publish(self.move_cmd)# Publish motion instructions
finally:
# Release the lock
self.lock.release()
# Sleep for 1/self.rate seconds
r.sleep()
# Subscribe to the callback function of the ROI topic =================
def set_cmd_vel(self, msg):
# Acquire a lock while we’re setting the robot speeds
self.lock.acquire()
try:
# If the ROI has a width or height of 0, we have lost the target
if msg.width == 0 or msg.height == 0:
self.target_visible = False # The target is invisible
return
# If the ROI stops updating this next statement will not happen
self.target_visible = True # The target is visible
# Compute the displacement of the ROI from the center of the image
# Horizontal offset of roi center and image center =======================
target_offset_x = msg.x_offset + msg.width / 2 – self.image_width / 2
# Calculate an offset ratio =============
try:
percent_offset_x = float(target_offset_x) / (float(self.image_width) / 2.0)
except:
percent_offset_x = 0
# Rotate the robot only if the displacement of the target exceeds the threshold
# Until the offset ratio is less than the threshold =====
if abs(percent_offset_x) > self.x_threshold:
# Set the rotation speed proportional to the displacement of the target
try:
speed = self.gain * percent_offset_x # Equivalent to a proportional controller
if speed < 0:
direction = -1 direction
else:
direction=1
#Rotation angular velocity
self.move_cmd.angular.z = -direction * max(self.min_rotation_speed,
min(self.max_rotation_speed, abs(speed)))
except:
self.move_cmd = Twist()
else:
# Otherwise stop the robot
self.move_cmd = Twist()
finally:
# Release the lock
self.lock.release()
def get_camera_info(self, msg):
self.image_width = msg.width
self.image_height = msg.height
def shutdown(self):
rospy.loginfo(“Stopping the robot…”)
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
if __name__ == ‘__main__’:
try:
ObjectTracker()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo(“Object tracking node terminated.”)