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)