/
follower.py
executable file
·169 lines (125 loc) · 6.49 KB
/
follower.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
#!/usr/bin/env python
"""
follower.py - Version 1.1 2013-12-20
Follow a "person" by tracking the nearest object in x-y-z space.
Based on the follower application by Tony Pratkanis at:
http://ros.org/wiki/turtlebot_follower
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
import rospy
from roslib import message
from sensor_msgs import point_cloud2
from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import Twist
from math import copysign
class Follower():
def __init__(self):
rospy.init_node("follower")
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# The dimensions (in meters) of the box in which we will search
# for the person (blob). These are given in camera coordinates
# where x is left/right,y is up/down and z is depth (forward/backward)
self.min_x = rospy.get_param("~min_x", -0.2)
self.max_x = rospy.get_param("~max_x", 0.2)
self.min_y = rospy.get_param("~min_y", -0.3)
self.max_y = rospy.get_param("~max_y", 0.5)
self.max_z = rospy.get_param("~max_z", 1.2)
# The goal distance (in meters) to keep between the robot and the person
self.goal_z = rospy.get_param("~goal_z", 0.6)
# How far away from the goal distance (in meters) before the robot reacts
self.z_threshold = rospy.get_param("~z_threshold", 0.05)
# How far away from being centered (x displacement) on the person
# before the robot reacts
self.x_threshold = rospy.get_param("~x_threshold", 0.05)
# How much do we weight the goal distance (z) when making a movement
self.z_scale = rospy.get_param("~z_scale", 1.0)
# How much do we weight left/right displacement of the person when making a movement
self.x_scale = rospy.get_param("~x_scale", 2.5)
# The maximum rotation speed in radians per second
self.max_angular_speed = rospy.get_param("~max_angular_speed", 2.0)
# The minimum rotation speed in radians per second
self.min_angular_speed = rospy.get_param("~min_angular_speed", 0.0)
# The max linear speed in meters per second
self.max_linear_speed = rospy.get_param("~max_linear_speed", 0.3)
# The minimum linear speed in meters per second
self.min_linear_speed = rospy.get_param("~min_linear_speed", 0.1)
# Slow down factor when stopping
self.slow_down_factor = rospy.get_param("~slow_down_factor", 0.8)
# Initialize the movement command
self.move_cmd = Twist()
# Publisher to control the robot's movement
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the point cloud
self.depth_subscriber = rospy.Subscriber('point_cloud', PointCloud2, self.set_cmd_vel, queue_size=1)
rospy.loginfo("Subscribing to point cloud...")
# Wait for the pointcloud topic to become available
rospy.wait_for_message('point_cloud', PointCloud2)
rospy.loginfo("Ready to follow!")
def set_cmd_vel(self, msg):
# Initialize the centroid coordinates point count
x = y = z = n = 0
# Read in the x, y, z coordinates of all points in the cloud
for point in point_cloud2.read_points(msg, skip_nans=True):
pt_x = point[0]
pt_y = point[1]
pt_z = point[2]
# Keep only those points within our designated boundaries and sum them up
if -pt_y > self.min_y and -pt_y < self.max_y and pt_x < self.max_x and pt_x > self.min_x and pt_z < self.max_z:
x += pt_x
y += pt_y
z += pt_z
n += 1
# If we have points, compute the centroid coordinates
if n:
x /= n
y /= n
z /= n
# Check our movement thresholds
if (abs(z - self.goal_z) > self.z_threshold):
# Compute the angular component of the movement
linear_speed = (z - self.goal_z) * self.z_scale
# Make sure we meet our min/max specifications
self.move_cmd.linear.x = copysign(max(self.min_linear_speed,
min(self.max_linear_speed, abs(linear_speed))), linear_speed)
else:
self.move_cmd.linear.x *= self.slow_down_factor
if (abs(x) > self.x_threshold):
# Compute the linear component of the movement
angular_speed = -x * self.x_scale
# Make sure we meet our min/max specifications
self.move_cmd.angular.z = copysign(max(self.min_angular_speed,
min(self.max_angular_speed, abs(angular_speed))), angular_speed)
else:
# Stop the rotation smoothly
self.move_cmd.angular.z *= self.slow_down_factor
else:
# Stop the robot smoothly
self.move_cmd.linear.x *= self.slow_down_factor
self.move_cmd.angular.z *= self.slow_down_factor
# Publish the movement command
self.cmd_vel_pub.publish(self.move_cmd)
def shutdown(self):
rospy.loginfo("Stopping the robot...")
# Unregister the subscriber to stop cmd_vel publishing
self.depth_subscriber.unregister()
rospy.sleep(1)
# Send an emtpy Twist message to stop the robot
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
if __name__ == '__main__':
try:
Follower()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("Follower node terminated.")