def __call__(self, dx, dy, dtheta, vcruise):
print('ssClient')
self.odom_sub = rospy.Subscriber('/t265/odom/sample', Odometry, self.odom_cb)
# wait for messages
time.sleep(.1)
self.client = SimpleActionClient('/bdbd/ssmotion', SsMotionAction)
print('waiting for action server')
self.client.wait_for_server()
sp = self.current_odom.pose.pose
st = self.current_odom.twist.twist
start_m = pose3to2(sp)
# ending delta is given relative to current robot frame
frame_m = (0.0, 0.0, 0.0)
start_r = transform2d(start_m, frame_m, start_m)
end_r = (start_r[0] + dx, start_r[1] + dy, start_r[2] + dtheta)
end_m = transform2d(end_r, start_m, frame_m)
ep = pose2to3(end_m)
et = Twist()
print('start pose: ' + fstr(pose3to2(sp)) + ' end pose: ' + fstr(pose3to2(ep)))
goal = SsMotionGoal()
goal.startPose = sp
goal.endPose = ep
goal.startTwist = st
goal.endTwist = et
goal.vcruise = vcruise
goal.skip = 4
goal.do_motor = True
print(goal)
self.client.send_goal(goal, feedback_cb=self.feedback_cb, done_cb=self.done_cb)
print('goal started, waiting for result')
def __call__(self, dt, source):
self.count += 1
s = source
if self.base_frame is None:
# save the first predicted trajectory and frame
self.base_pxj = source.pxwj.copy()
self.base_pyj = source.pywj.copy()
self.base_frame = source.noww_pose_map
self.base_pxrj = source.pxrj.copy()
self.base_pyrj = source.pyrj.copy()
#print(gstr({'base_pxj': self.base_pxj, 'base_pyj': self.base_pyj}))
if hasattr(s, 'left') and hasattr(s, 'right') and hasattr(s, 'tee'):
self.history_lefts.append(s.left)
self.history_rights.append(s.right)
self.history_tees.append(s.tee)
fig = self.fig
if hasattr(s, 'tees'):
tees = s.tees
else:
tees = [0.0]
tt = 0.0
for i in range(1, len(s.lefts)):
tt += dt
tees.append(tt)
fig.clf()
plt1 = fig.add_subplot(131)
#plt1.axis([0.0, tfPath.lrs[-1]['t'], -1.5, 1.5])
plt2 = fig.add_subplot(132)
plt3 = fig.add_subplot(133)
#if axis3 is not None:
# plt3.axis(axis3)
plt2.axis('equal')
plt1.plot(tees, s.lefts)
plt1.plot(tees, s.rights)
plt1.plot(self.history_tees, self.history_lefts, 'g+')
plt1.plot(self.history_tees, self.history_rights, 'r+')
plt1.set_title('left,right')
# plt1.plot(s.tees, s.omegaj)
# Transform to base_frame coordinates
pxj_base = []
pyj_base = []
for i in range(len(s.pxwj)):
p_wheel = (s.pxwj[i], s.pywj[i], s.thetaj[i])
p_base = transform2d(p_wheel, s.noww_pose_map, self.base_frame)
pxj_base.append(p_base[0])
pyj_base.append(p_base[1])
#print(gstr({'base_frame':self.base_frame, 'pxj_base': pxj_base, 'pyj_base': pyj_base}))
noww_pose_base = transform2d(s.noww_pose_map, (0., 0., 0.),
self.base_frame)
self.history_pxwj_base.append(noww_pose_base[0])
self.history_pywj_base.append(noww_pose_base[1])
nowr_pose_base = transform2d(s.nowr_pose_map, (0., 0., 0.),
self.base_frame)
self.history_pxrj_base.append(nowr_pose_base[0])
self.history_pyrj_base.append(nowr_pose_base[1])
if self.history_rate and self.count % self.history_rate == 1:
self.plan_history.append((pxj_base, pyj_base))
else:
#plt2.plot(pxj_base, pyj_base)
pass
if self.history_rate:
for plan in self.plan_history:
plt2.plot(plan[0], plan[1])
plt2.plot(self.base_pxj, self.base_pyj)
plt2.plot(self.base_pxrj, self.base_pyrj)
#plt2.plot(pxj_base, pyj_base)
plt2.plot(self.history_pxwj_base, self.history_pywj_base, 'r+')
plt2.plot(self.history_pxrj_base, self.history_pyrj_base, 'g+')
plt2.set_title('px vs py')
#plt3.plot(tees, s.pxj)
#plt3.plot(tees, s.pyj)
#plt3.set_title('px,py vs t')
plt.pause(.001)
source = Object()
source.pxwj = []
source.pywj = []
source.pxrj = []
source.pyrj = []
source.thetaj = []
source.tees = tees
source.lefts = lefts
source.rights = rights
# the base frame (zero at start of plan) is the wheel location
for pose in poses:
source.pxwj.append(pose[0])
source.pywj.append(pose[1])
source.thetaj.append(pose[2])
robot_pose_map = transform2d(pp.robot_w, pose, zero3)
source.pxrj.append(robot_pose_map[0])
source.pyrj.append(robot_pose_map[1])
source.noww_pose_map = zero3
source.nowr_pose_map = pp.robot_w
pathPlot(dt, source)
# now use the dynamic/control model
pose_m = source.nowr_pose_map
twist_m = zero3
next_left = lefts[0]
next_right = rights[0]
Q = np.identity(6)
for i in range(len(Qfact)):
Q[i][i] *= Qfact[i]
pass
source = Object()
source.pxwj = []
source.pywj = []
source.pxrj = []
source.pyrj = []
source.thetaj = []
source.tees = tees
source.lefts = lefts
source.rights = rights
# the base frame (zero_map at start of plan) is the robot location. But the path
# plan is in plan coordinates, with 0 being the wheel start.
pose_m = zero3
frame_m = zero3
wheel_m = transform2d(pp.wheel_r, pose_m, zero3)
source.noww_pose_map = wheel_m
source.nowr_pose_map = pose_m
for pose in poses:
wheel_p = pose
wheel_m = transform2d(wheel_p, pp.frame_p, frame_m)
robot_m = transform2d(pp.robot_w, wheel_m, frame_m)
source.pxwj.append(wheel_m[0])
source.pywj.append(wheel_m[1])
source.thetaj.append(wheel_m[2])
source.pxrj.append(robot_m[0])
source.pyrj.append(robot_m[1])
pathPlot(dt, source)
# now use the dynamic/control model
twist_m = zero3
pass
source = Object()
source.pxwj = []
source.pywj = []
source.pxrj = []
source.pyrj = []
source.thetaj = []
source.tees = tees
source.lefts = lefts
source.rights = rights
# the base frame (zero_map at start of plan) is the robot location. But the path
# plan is in plan coordinates, with 0 being the wheel start.
pose_m = zero3
frame_m = zero3
wheel_m = transform2d(pp.wheel_r, pose_m, zero3)
source.noww_pose_map = wheel_m
source.nowr_pose_map = pose_m
for pose in poses:
wheel_p = pose
wheel_m = transform2d(wheel_p, pp.frame_p, frame_m)
robot_m = transform2d(pp.robot_w, wheel_m, frame_m)
source.pxwj.append(wheel_m[0])
source.pywj.append(wheel_m[1])
source.thetaj.append(wheel_m[2])
source.pxrj.append(robot_m[0])
source.pyrj.append(robot_m[1])
pathPlot(dt, source)
# now use the dynamic/control model
twist_m = zero3
def __call__(self, dt, source):
self.count += 1
if self.base_frame is None:
# save the first predicted trajectory and frame
self.base_pxj = source.pxj.copy()
self.base_pyj = source.pyj.copy()
self.base_frame = source.now_pose_map
fig = self.fig
s = source
if hasattr(s, 'tees'):
tees = s.tees
else:
tees = [0.0]
tt = 0.0
for i in range(1, len(s.lefts)):
tt += dt
tees.append(tt)
fig.clf()
plt1 = fig.add_subplot(131)
#plt1.axis([0.0, tfPath.lrs[-1]['t'], -1.5, 1.5])
plt2 = fig.add_subplot(132)
plt3 = fig.add_subplot(133)
#if axis3 is not None:
# plt3.axis(axis3)
plt2.axis('equal')
#plt1.plot(tees, s.lefts)
#plt1.plot(tees, s.rights)
#plt1.set_title('left,right')
# plt1.plot(s.tees, s.omegaj)
# Transform to base_frame coordinates
pxj_base = []
pyj_base = []
for i in range(len(s.pxj)):
p_robot = (s.pxj[i], s.pyj[i], s.thetaj[i])
p_base = transform2d(p_robot, s.now_pose_map, self.base_frame)
pxj_base.append(p_base[0])
pyj_base.append(p_base[1])
now_pose_base = transform2d(s.now_pose_map, (0., 0., 0.),
self.base_frame)
self.history_pxj_base.append(now_pose_base[0])
self.history_pyj_base.append(now_pose_base[1])
self.history_wheelx.append(s.now_pose_wheel[0])
self.history_wheely.append(s.now_pose_wheel[1])
if self.count % self.history_rate == 0:
self.plan_history.append((pxj_base, pyj_base))
else:
plt2.plot(pxj_base, pyj_base)
for plan in self.plan_history:
plt2.plot(plan[0], plan[1])
#plt2.plot(self.base_pxj, self.base_pyj)
plt2.plot(pxj_base, pyj_base)
plt2.plot(self.history_pxj_base, self.history_pyj_base, 'r+')
#plt2.plot(self.history_wheelx, self.history_wheely, 'g+')
plt2.set_title('px vs py')
#plt3.plot(tees, s.pxj)
#plt3.plot(tees, s.pyj)
#plt3.set_title('px,py vs t')
plt.pause(.001)
maxN = 50
rate = 20
callback_dt = 1. / rate
min_lr = 0.15
MAX_STEPS = 10
integralK = 5 * dt
rospy.init_node('NewRaph', log_level=rospy.DEBUG)
odom = Odom()
while odom.get_count() < 20:
odom.process()
rospy.sleep(0.001)
zero3 = (0.0, 0.0, 0.0)
(start_pose_map, start_twist_robot) = odom.get_odom()
target_pose_map = transform2d(target_pose, start_pose_map, zero3)
#print(gstr({'start_pose_map': start_pose_map, '\nstart_twist_robot': start_twist_robot, '\ntarget_pose_map': target_pose_map}))
# Main Loop
rosrate = rospy.Rate(20)
first_time = True
now_pose_old = zero3
progress_dt = 0.0
motor_pub = rospy.Publisher('/bdbd/motors/cmd_raw',
MotorsRaw,
queue_size=10)
pathPlot = PathPlot(history_rate=5)
(tees, lefts, rights, max_segments, pp, plan_time, vxres,
omegas) = static_plan(dt,
start_pose=zero3,
rightOld = right
# control step
(v_new, o_new) = pp.controlStep(dt, pose_m, twist_r)
(left, right) = motor(v_new, o_new)
'''
# introduce an error
left *= random.gauss(0.95, 0.05)
right *= random.gauss(0.9, 0.05)
'''
#(left, right) = speeds[ii]['lr']
# speeds[]['point'] is the progression of the wheels starting at (0.0, 0.0, 0.0), ie in frame_p
# get wheels_m
plan_w_p = speeds[ii]['point']
plan_w_m = transform2d(plan_w_p, pp.frame_p, pp.frame_m)
# plan_w_m is also the w frame in the plan. Get robot position in that frame
plan_r_m = transform2d(pp.robot_w, plan_w_m, pp.frame_m)
real_wheel_m = transform2d(pp.wheel_r, pose_m, pp.frame_m)
print(' ')
print(
fstr({
't': tt,
'va': pp.va,
'v_new': v_new,
'vhat_a': pp.vhata,
'vhat_new': pp.vhat_new,
'vhat_plan': pp.vhat_plan,
'oa': pp.oa,
ByM = 8.0 ''' # free parameters Adegrees = 0.0 Bdegrees = 0.0 xA = -0.13 yA = 0.0 thetaMDegrees = 0.0 AxM = 0.0 AyM = 0.0 BxM = -0.13 ByM = 0.0 thetaA = (thetaMDegrees - Adegrees) * D_TO_R Atheta = Adegrees * D_TO_R Btheta = Bdegrees * D_TO_R poseA = (xA, yA, thetaA) frameA = (AxM, AyM, Atheta) frameB = (BxM, ByM, Btheta) pA = transform2d(poseA, frameA, frameB) print(fstr(pA)) pM = transform2d(poseA, frameA, (0., 0., 0.)) print(fstr(pM))
# scratchpad for various simple experiments
from bdbd_common.geometry import pose3to2, pose2to3, D_TO_R, transform2d
from bdbd_common.utils import fstr
from bdbd_common.pathPlan2 import PathPlan
startPose3 = pose2to3((0.0, 0.0, 0.0))
endPose3 = pose2to3((.4, .1, 30. * D_TO_R))
pp = PathPlan()
pp.start(startPose3, endPose3)
for seg in pp.path:
print(fstr(seg, '8.5f'))
for seg in pp.path:
# The control model operates in the robot frame, relative to the plan. So we need the deviation
# of the actual location from the plan location, in the robot frame.
near_wheel_m = transform2d(seg['end'], pp.frame_p, pp.frame_m)
# near_wheel_m is the current origin of the wheel frame for the nearest point
near_robot_m = transform2d(pp.robot_w, near_wheel_m, pp.frame_m)
print(fstr(near_robot_m, '8.5f'))
#print(fstr(pose3to2(pose3), '10.7f'))
