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 goal_cb(self):
rospy.loginfo('accept new goal')
goal = self._actionServer.accept_new_goal()
self.goal = goal
self.last_time = None
self.skip = goal.skip
self.dt = self.skip * ODOM_RATE
if goal.do_motor:
self.motor_pub = rospy.Publisher('/bdbd/motors/cmd_raw',
MotorsRaw,
queue_size=10)
else:
self.motor_pub = None
filedir = os.path.dirname(os.path.abspath(__file__)) + '/bddata'
lrd = LrDynamicsStore(filedir)
lr_model = lrd.get_lr_model()
self.ssControl = SsControl(lr_model=lr_model)
# convert to 2D coordinates x, y, theta. The pose coordinates are in the frame which will
# be considered the base frame for these calculations.
start_m = pose3to2(goal.startPose)
end_m = pose3to2(goal.endPose)
self.pp = static_plan(start_pose=start_m,
target_pose=end_m,
start_twist=twist3to2(goal.startTwist),
target_twist=twist3to2(goal.endTwist),
approach_rho=0.20,
min_rho=0.10,
cruise_v=goal.vcruise,
vslow=self.vslow,
u=0.50,
details=True)
self.replan_time = rospy.get_time()
rospy.loginfo(
'Executing changePose with goal x: {:6.3f} y: {:6.3f} theta: {:6.3f}'
.format(*end_m))
# start executing the action, driven by odometry message receipt
self.odom_sub = rospy.Subscriber('/t265/odom/sample',
Odometry,
self.odom_cb,
tcp_nodelay=True)
def __call__(self, dx, dy, dphi, vcruise):
print('changePoseClient')
self.odom_sub = rospy.Subscriber('/t265/odom/sample', Odometry,
self.odom_cb)
# wait for messages
time.sleep(.1)
self.client = actionlib.SimpleActionClient('/bdbd/changepose',
ChangePoseAction)
self.client.wait_for_server()
sp = self.current_odom.pose.pose
st = self.current_odom.twist.twist
start_m = pose3to2(sp)
end_m = (start_m[0] + dx, start_m[1] + dy, start_m[2] + dphi)
ep = pose2to3(end_m)
et = Twist()
print('start pose: ' + fstr(pose3to2(sp)) + ' end pose: ' +
fstr(pose3to2(ep)))
goal = ChangePoseGoal(sp, ep, st, et, vcruise)
self.client.send_goal(goal,
feedback_cb=self.feedback_cb,
done_cb=self.done_cb)
print('waiting for result')
def process(self):
if self.queue.empty():
return
odometry = self.queue.get()
new_factor = 0.10
pose_m = pose3to2(odometry.pose.pose)
twist3 = odometry.twist.twist
twist_r = (twist3.linear.x, twist3.linear.y, twist3.angular.z)
for i in range(3):
self.poses[i] = (
1. - new_factor) * self.poses[i] + new_factor * pose_m[i]
self.twists[i] = (
1. - new_factor) * self.twists[i] + new_factor * twist_r[i]
self.count += 1
def feedback_cb(self, f):
self.fcount += 1
(left, right) = self.motor(f.vnew, f.onew)
lag = rospy.get_time() - f.rostime
self.motor_pub.publish(left, right)
pose_m = pose3to2(self.current_odom.pose.pose)
#if self.fcount % 5 == 0:
print('tt: {:6.3f} '.format(f.tt) + fstr({
'vnew': f.vnew,
'onew': f.onew,
'left': left,
'right': right,
'psi deg': f.psi / D_TO_R,
'fraction': f.fraction,
'dy': f.dy,
'pose_m': pose_m,
'lagms': lag * 1000.,
#'feedback time': time.time() - start
}))
#print('rostime: {:15.3f}'.format(rospy.get_time()))
return
def odom_cb(self, odometry):
self.current_odom = odometry
pose_m = pose3to2(odometry.pose.pose)
print(pose_m)
return
def odom_cb(self, odometry):
pose_m = pose3to2(odometry.pose.pose)
twist_r = twist3to2(odometry.twist.twist)
self.count += 1
self.psum[0] += pose_m[0]
self.psum[1] += pose_m[1]
self.psum[2] += math.cos(pose_m[2])
self.psum[3] += math.sin(pose_m[2])
for i in range(3):
self.tsum[i] += twist_r[i]
if self.count % self.skip != 0:
return
then = float(odometry.header.stamp.secs +
1.0e-9 * odometry.header.stamp.nsecs)
if self.last_time is None:
self.last_time = then
return
pasum = []
tasum = []
for i in range(3):
pasum.append(self.psum[i] / self.skip)
tasum.append(self.tsum[i] / self.skip)
pasum.append(self.psum[3] / self.skip)
self.psum = [0.0, 0.0, 0.0, 0.0]
self.tsum = [0.0, 0.0, 0.0]
pose_m = (pasum[0], pasum[1], math.atan2(pasum[3], pasum[2]))
dt = then - self.last_time
self.tt += dt
self.last_time = then
lag = rospy.get_time() - then
#rospy.loginfo('ssControl: ' + fstr({'lag': lag, 'dt': self.dt, 'tt': self.tt, 'pose_m': pose_m, 'twist_r': tasum}))
(lr, ctl_state) = self.ssControl(self.dt, pose_m, tasum, self.pp)
eps = ctl_state['eps']
pos_err = math.sqrt(0.33 * (eps[3]**2 + eps[4]**2 + eps[5]**2))
ssResults = SsResults()
# damping, used mostly when replanning
self.new_factor = min(1.0, self.new_factor + .04)
ssResults.left = self.new_factor * lr[0] + (
1. - self.new_factor) * self.last_left
ssResults.right = self.new_factor * lr[1] + (
1. - self.new_factor) * self.last_right
#print(fstr({'new_factor': self.new_factor, 'lr[0]': lr[0], 'last_left': self.last_left, 'left': ssResults.left}))
self.last_left = ssResults.left
self.last_right = ssResults.right
ssResults.rms_err = ctl_state['rms_err']
ssResults.eps = ctl_state['eps']
ssResults.pose_m = pose_m
ssResults.twist_r = twist_r
ssResults.fraction = ctl_state['vv']['fraction']
if self.motor_pub:
self.motor_pub.publish(ssResults.left, ssResults.right)
self.ssResultsPub.publish(ssResults)
self._actionServer.publish_feedback(ssResults)
self.results = ssResults
#(pose_m, twist_m, twist_r) = dynamicStep(lr, dt, pose_m, twist_m)
lrc = ctl_state['lr_corr']
lrb = ctl_state['lr_base']
#rospy.loginfo(fstr({'pos_err': pos_err, 'lag': lag, 'ctl_left': lr[0], 'ctl_right': lr[1]}))
#rospy.loginfo(fstr({'L0': lrb[0], 'R0': lrb[1], 'corr_left': lrc[0], 'corr_right': lrc[1]}))
if ssResults.fraction > 0.999 and self._actionServer.is_active():
rospy.loginfo('fraction > 0.999, finishing goal')
self.finish()
# replanning
time_since_replan = rospy.get_time() - self.replan_time
if ssResults.fraction < 0.90 and pos_err > 0.10 and self.replan_count < 4 and time_since_replan > 2.0:
self.replan_count += 1
self.replan_time = rospy.get_time()
self.new_factor = 0.0
rospy.loginfo('pos_err exceeds limits, replanning path')
self.pp = static_plan(start_pose=pose_m,
target_pose=pose3to2(self.goal.endPose),
start_twist=twist_r,
target_twist=twist3to2(self.goal.endTwist),
approach_rho=0.20,
min_rho=0.10,
cruise_v=self.goal.vcruise,
vslow=self.vslow,
u=0.50,
details=True)