GeezerVison is showing github.com — backend/coderbot/motion.py at develop · CoderBotOrg/backend

Expand file tree

motion.py

Copy path

More file actions

Latest commit

History

241 lines (205 loc) · 9.29 KB

motion.py

Copy path

File metadata and controls

241 lines (205 loc) · 9.29 KB

Raw

Copy raw file

Download raw file

Open symbols panel

Edit and raw actions

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

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

############################################################################

# CoderBot, a didactical programmable robot.

# 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.

# You should have received a copy of the GNU General Public License along

# with this program; if not, write to the Free Software Foundation, Inc.,

# 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

############################################################################

from time import time

import logging

import numpy as np

import cv2

from cv import image

from coderbot import CoderBot

from camera import Camera

from config import Config

lk_params = dict(winSize=(15, 15),

maxLevel=2,

criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

feature_params = dict(maxCorners=500,

qualityLevel=0.3,

minDistance=7,

blockSize=7)

PI_CAM_FOV_H_DEG = 53.0

PI_CAM_FOV_V_CM = 100.0

class Motion:

# pylint: disable=too-many-instance-attributes

def __init__(self):

self.bot = CoderBot.get_instance()

self.cam = Camera.get_instance()

self.track_len = 2

self.detect_interval = 5

self.tracks = []

self.frame_idx = 0

self.ts = time()

self.frame_gray = None

self.prev_gray = None

self.running = False

self.delta_power = 0.0

self.delta_dist = 0.0

self.target_dist = 0.0

self.delta_angle = 0.0

self.target_angle = 0.0

cfg = Config.get()

self.power_angles = [[15, (int(cfg.get("move_power_angle_1")), -1)],

[4, (int(cfg.get("move_power_angle_2")), 0.05)],

[1, (int(cfg.get("move_power_angle_3")), 0.02)],

[0, (0, 0)]]

self.image_width = 640 / int(cfg.get("cv_image_factor"))

self.image_heigth = 480 / int(cfg.get("cv_image_factor"))

self.transform = image.Image.get_transform(self.image_width)

_motion = None

@classmethod

def get_instance(cls):

if not cls._motion:

cls._motion = Motion()

return cls._motion

def move(self, dist):

self.delta_dist = 0.0

self.delta_angle = 0.0

self.target_dist = dist

self.target_angle = 0.0

self.delta_power = 0.0

self.loop_move()

def turn(self, angle):

self.delta_dist = 0.0

self.delta_angle = 0.0

self.target_dist = 0.0

self.target_angle = angle

self.loop_turn()

def stop(self):

self.running = False

def loop_move(self):

self.running = True

while self.running:

frame = self.cam.get_image()

self.frame_gray = frame.grayscale()

if len(self.tracks) < 2 or self.frame_idx % self.detect_interval == 0:

self.find_keypoints(self.frame_gray, self.tracks)

if self.tracks and self.prev_gray is not None:

self.track_keypoints(self.prev_gray, self.frame_gray, self.tracks)

if self.tracks:

delta_angle, delta_dist = self.calc_motion()

self.running = self.running and self.bot_move(self.target_dist, delta_dist, delta_angle)

self.frame_idx += 1

self.prev_gray = self.frame_gray

self.bot.stop()

def loop_turn(self):

self.running = True

while self.running:

frame = self.cam.get_image()

self.frame_gray = frame.grayscale()

if len(self.tracks) < 2 or self.frame_idx % self.detect_interval == 0:

self.find_keypoints(self.frame_gray, self.tracks)

if self.tracks and self.prev_gray is not None:

self.track_keypoints(self.prev_gray, self.frame_gray, self.tracks)

if self.tracks:

delta_angle, delta_dist = self.calc_motion()

self.running = self.running and self.bot_turn(self.target_angle, delta_angle)

self.frame_idx += 1

self.prev_gray = self.frame_gray

self.bot.stop()

def find_keypoints(self, image_gray, tracks):

#print "find_keypoints"

ts = time()

mask = np.zeros_like(image_gray._data)

mask[:] = 255

#for x, y in [np.int32(tr[-1]) for tr in tracks]:

# cv2.circle(mask, (x, y), 5, 0, -1)

p = cv2.goodFeaturesToTrack(image_gray._data, mask=mask, **feature_params)

if p is not None:

for x, y in np.float32(p).reshape(-1, 2):

tracks.append([(x, y)])

#print "fk: ", str(time() - ts)

def track_keypoints(self, prev_image, cur_image, tracks):

#print "track_keypoints"

#ts = time()

img0, img1 = prev_image._data, cur_image._data

p0 = np.float32([tr[-1] for tr in tracks]).reshape(-1, 1, 2)

p1, st, err = cv2.calcOpticalFlowPyrLK(img0, img1, p0, None, **lk_params)

p0r, st, err = cv2.calcOpticalFlowPyrLK(img1, img0, p1, None, **lk_params)

d = abs(p0-p0r).reshape(-1, 2).max(-1)

good = d < 1

#good = st

#print "tk: ", str(time() - ts)

new_tracks = []

for tr, (x, y), good_flag in zip(tracks, p1.reshape(-1, 2), good):

if not good_flag:

continue

tr.append((x, y))

if len(tr) > self.track_len:

del tr[0]

new_tracks.append(tr)

#cv2.circle(self.vis, (x, y), 2, (0, 255, 0), -1)

#print "initial tp: ", len(self.tracks), " current tp: ", len(new_tracks)

#print len(new_tracks), len(tracks)

tracks[:] = new_tracks[:]

if tracks is False:

logging.warning("lost ALL tp!")

self.bot.stop()

#exit(0)

#cv2.polylines(self.vis, [np.int32(tr) for tr in self.tracks], False, (0, 255, 0))

def calc_motion(self):

vectors = self.tracks

avg_delta_x = 0.0

avg_delta_y = 0.0

count = 0

vectors_t = []

for vect in vectors:

if len(vect) > 1:

count += 1

vectors_t.append(vect[-2])

vectors_t.append(vect[-1])

avg_delta_x += (vect[-1][0] - vect[-2][0])

vectors_t = vectors_t[:min(len(vectors_t), 20)] #max 10 keypoints

#avg_delta_x_t = 0.0

if vectors_t:

vectors_t = image.Image.transform(vectors_t, self.transform)

for v in vectors_t.reshape(-1, 2, 2):

avg_delta_y += (v[1][1] - v[0][1])

#avg_delta_x_t += v[1][0] - v[0][0]

#avg_delta_x_t = avg_delta_x_t / (vectors_t.shape[0] / 2)

#for v in vectors_t.reshape(-1, 2, 2):

#if abs(v[1][0] - v[0][0] - avg_delta_x_t) > 2:

#print "this is an obstacle: ", str(v[1]), " delta_x: ", v[1][0] - v[0][0]

if count > 0:

avg_delta_x = (avg_delta_x / count)

avg_delta_y = (avg_delta_y / (vectors_t.shape[0] / 2))

self.delta_angle -= (avg_delta_x * PI_CAM_FOV_H_DEG) / self.image_width

self.delta_dist += (avg_delta_y * PI_CAM_FOV_V_CM) / self.image_heigth

#print "count: ", count, "delta_a: ", self.delta_angle, " avg_delta_x: ", avg_delta_x, " delta_y: ", self.delta_dist, " avg_delta_y: ", avg_delta_y

#cv2.line(self.vis, (int(80+deltaAngle),20), (80,20), (0, 0, 255))

#cv2.putText(self.vis, "delta: " + str(int((self.deltaAngle*53.0)/160.0)) + " avg_delta: " + str(int(((avg_delta_x*53.0)/160.0))),

#(0,20), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (0, 0, 255))

return self.delta_angle, self.delta_dist

def bot_turn(self, target_angle, delta_angle):

run = True

sign = (target_angle - delta_angle) / abs(target_angle - delta_angle)

logging.info("abs delta: %s sign delta: %s", str(abs(target_angle - delta_angle)), str(sign))

for p_a in self.power_angles:

if abs(target_angle - delta_angle) > p_a[0] and self.running:

#print "pow: ", p_a[1][0], " duration: ", p_a[1][1]

self.bot.motor_control(sign * p_a[1][0], -1 * sign * p_a[1][0], p_a[1][1])

run = p_a[1][0] > 0 #stopped

break

return run

def bot_move(self, target_dist, delta_dist, delta_angle):

base_power = 100 * (target_dist/abs(target_dist))

self.delta_power += (delta_angle * 0.01)

logging.info("base power: %s delta power: %s delta_dist: %s target_dist: %s", str(base_power), str(self.delta_power), str(delta_dist), str(target_dist))

if abs(delta_dist) < abs(target_dist):

self.bot.motor_control(min(max(base_power - self.delta_power, -100), 100), min(max(base_power + self.delta_power, -100), 100), -1)

else:

self.bot.stop()

return abs(delta_dist) < abs(target_dist)